Kinematics Féli Monasterio-Huelin, Álvaro Gutiérre & Blanca Larraga September 5, 2018 Contents Contents 1 List of Figures 1 List of Tables 2 Acronm list 3 1 Degrees of freedom and kinematic chains of rigid bod mechanisms 4 2 Si degrees of freedom robot 7 3 Forward kinematics of position and orientation 8 4 Hand orientation vector (a, n, s) 11 5 Kinematic decoupling 12 6 Inverse kinematics of position 12 7 Inverse kinematics of orientation 14 8 Kinematic conditions for q 4 = 0 16 A Vector operations and rotation matrices 18 B Solution to the inverse kinematics of position 20 List of Figures 1.1 Two DOFs planar mechanisms.............................. 4 1.2 Zero DOFs planar mechanism.............................. 4 1.3 One DOF plan mechanism: simple pendulum...................... 5 1.4 3 DOFs three-dimensional mechanism.......................... 6 2.1 6 DOFs robot....................................... 7 3.1 Local reference sstems.................................. 8 3.2 Local reference sstems and kinematic pair (2, 3).................... 9 4.1 Hand orientation and projection over plane XY..................... 11 5.1 Kinematic decoupling................................... 12 6.1 4 DOFs mechanism.................................... 13 6.2 Two different solutions to the same inverse kinematics problem of position...... 14 7.1 RS I H configuration with RS II A............................... 15 7.2 Solutions to the inverse kinematics problem of orientation............... 15 8.1 Hand orientation with q 4 = 0............................... 17 B.1 4 DOFs robot....................................... 20 1
List of Tables 2
Acronm List DOF: Degree of freedom SO: Special Orthogonal group (also called rotation group) SE: Special Euclidean group S i : Local Reference Sstem of ever i-segment o i : Origin of ever S i õ i : Point o i declared in the reference sstem S i 1 prior to the rotation H: Hand A: Arm C: Wrist center Q: Hand s end 3
1 Degrees of freedom and kinematic chains of rigid bod mechanisms The concept of Degree of Freedom (DOF) has different meanings. In this document, it will be eclusivel used in the sense of the mobilit of a mechanism. This mechanism is considered to be composed of a set of rigid bodies connected b an tpe of joint, that is, creating a kinematic chain of rigid bodies. l 2 l 2 l 3 = l 1 l 1 l 1 l 4 (a) Two DOFs planar serial mechanism. (b) Five bars Two DOFs planar parallel mechanism. Figure 1.1: Two DOFs planar mechanisms. Figure 1.1 shows two planar mechanisms of two DOFs. Figure 1.1a shows a serial mechanism as an open kinematic chain while Figure 1.1b shows a parallel mechanism as a closed kinematic chain. Serial mechanisms are alwas composed of open kinematic chains, while in parallel mechanisms there is, at least, one closed kinematic chain. Mechanisms are defined as parallel when the end effector (l 2 in Figure 1.1b) is connected to more than one independent mechanism: l 1 is one independent mechanism and l 3 and l 4 form another one. An advantage of parallel mechanisms is that motors can be attached to a common surface, while in serial mechanisms motors must be attached to each joint. As a consequence, parallel mechanisms are tpicall lighter than serial ones and reduce the inherent fleibilit of a rigid bar. A kinematic pair is a connection between two rigid bodies that imposes constraints on their relative movement. A kinematic pair is defined b its DOFs, but it is also tpical to talk about the DOFs of the joint of the kinematic par. Therefore, it is the same to talk about a rotational joint of 1 DOF than a kinematic pair with a rotational joint of 1 DOF. l 2 l 1 Figure 1.2: Zero DOFs planar mechanism. The mobilit of a mechanism is a kinematic concept that indicates the capacit that the mechanism has to perform infinitesimal movements. The number of DOFs is the minimum number of independent kinematic variables that allows a mechanism to move. For eample, the mechanism of Figure 1.2 consists of a closed planar kinematic chain made up of two bars joined to the ground b 3 rotational joints. It has no mobilit, so the number of DOFs equals ero. The number of DOFs of a rigid bod that moves freel in the three-dimensional Cartesian space equals 6: three for the position in R 3 and three for the orientation in SO(3) (creating the SE(3) = 4
R 3 SO(3) space, called group of spatial rigid transformations which dimension is D = 6). If movements are limited in such a wa that it remains in the plane, the number of DOFs is reduced to 3: two for the position in R 2 and one for the orientation in SO(2) (creating the SE(2) = R 2 SO(2) space, called group of planar rigid transformations which dimension is D = 3). Independentl of how the plane is limited, the sstem can be defined as three independent equations that onl depend on the position and orientation. The rotational space has changed from SO(3), with D = 3, to SO(2), with D = 1 and the Cartesian space has changed from R 3 to R 2. Moreover, if the sstem is limited to perform rotations around a perpendicular ais to the plane (rotation kinematic pair R) or translation motion following a line contained in the plane (translational or prismatic kinematic pair P ), the number of DOFs will be reduced to 1. On the first case, this is because it cannot perform an translational movement, so it can onl perform rotations on a subspace of D = 1, that is, a circumference. On the second case, this is because it cannot perform an rotational movement so it can onl perform translations on a subspace of D = 1, that is, a line. Q a Figure 1.3: One DOF plan mechanism: simple pendulum. Figure 1.3 shows the first case, a simple pendulum. It is a 1 DOF mechanism where the center of mass (point Q) can onl be moved in a circumference of constant radio. Because the movement is limited to the plane, just one rotation is allowed to change the orientation of a vector, Therefore, there are two constraints with respect to the position in the Cartesian plane. In general, it is not eas to know the number of constraints of a three-dimensional mechanism. However, if a mechanical sstem in R 3 has a set of N rigid bodies and m constraints than onl depend on the position r i R 3, i = 1... N and the orientation R i SO(3) defined b integrable and independent vector functions such as g k (r, R) = 0, k = 1... m. Then, the Implicit Function Theorem calculates the number of DOFs as: n = 6N m (1.1) As an eample, a mechanism as the one shown in Figure 1.1a, which has 2 segments (without taking into account the bar in the ground), N = 2, and two rotational joints (of 1 DOF each), has 2 DOFs (n = 2). This is because the number of constraints (in the three-dimensional space R 3 ) of each joint is 5, therefore m = 10. In the literature, several studies have focused on the calculation of the number of DOFs of a rigid bod attending to the number of elements, number of joints or kinematic pairs and the DOFs of ever joint or kinematic pair. The Grübler-Kutbach mobilit criteria is the most known. However, the reader must take into account that it does not calculates correctl the number of DOFs for ever mechanism. The Grübler-Kutbach equation is written as follows: 5
n = D(N 1 j) + j f i (1.2) where D is the space dimension in which the mechanism is located (D = 6 in a three-dimensional space and D = 3 in the plane), N is the number of elements in which the anchor to the ground is counted, j is the number of joints and f i is the number of DOFs of the i th joint. Equation 1.2 is valid for man mechanisms (simple open (j = N 1) and closed (j = N ) kinematic chains included). The closed kinematic chain planar mechanism (D = 3) of Figure 1.1b has n = 2 DOFs because it has five segments (taking into account the anchor to the ground), N = 5, and five rotational joints, j = 5, of 1 DOF each, f i = 1. Figure 1.4 shows a 3 DOFs serial mechanism as a simple open kinematic chain which satisfies the Grübler-Kutbach equation: D = 6, N = 4, j = 3 and f i = 1 with i = 1, 2, 3. i=1 3 3 P l 3 q 3 2 1 2 l 2 q 2 l 1 1 l 0 q 1 P Figure 1.4: 3 DOFs three-dimensional mechanism. Ever DOF is associated to q i, i = 1... n, which will be named in this tet as generalied coordinate or joint coordinate. 6
2 Si degrees of freedom robot q 6 q 5 q 3 q 4 q 2 q 1 Figure 2.1: 6 DOFs robot. Figure 2.1 shows the robot to be studied in this document. It has 6 DOFs {q 1, q 2, q 3, q 4, q 5, q 6 }. In Section 8 a specific case is studied, where the movement of q 4 is restricted with the objective of analing a 5 DOFs robot. { π The specific generalied coordinates values shown in Figure 2.1 are 3, 5π 36, π 9, 3π 4, π 4, π }. 4 Moreover, Figure 2.1 shows the rotation aes of each segment relative to the previous one. Section 3 defines more ehaustivel the reference sstem of ever segment. In general, an set of generalied coordinates can be selected as long as the are independent of each other. In fact, for the robot to be studied in this document, ever generalied coordinate should be an invertible linear combination of the ones defined in this document. It is not mandator that the generalied coordinates are relative angles between adjacent segments. For eample, the third generalied coordinate could have been defined as θ 3 = q 2 + q 3, and the set {q 1, q 2, θ 3, q 4, q 5, q 6 } will still represent the generalied coordinates of the same robot. In practice, the selection of one sstem or another depends on the tpe of robot and the stud to be done. The selection of Figure 2.1 is done because the actuators will be located at ever joint, so it is the most convenient representation from the control point view. In the remainder of this document, the first three generalied coordinates {q 1, q 2, q 3 } will be called the arm or elbow and the last three ones {q 4, q 5, q 6 } will be called the hand or spherical wrist. Section 5 will show how a robot composed of these two structures simplifies the inverse kinematics problem; that is, the problem of obtaining the generalied coordinates when the position and rotation, in the Cartesian space, of the hand s end are known. In an robotic problem related to a task of grasping, transporting, pushing, etc. and provided that there are no eteroceptive sensors (such as vision, touch, etc., that is, sensors relative to the outside world of the robot), it will be mandator to solve this problem. That is because the robot will start with a specific configuration S q0 and should move to another one S qf. Nonetheless, the trajector generation problem and the control problem should also be solved. Finall, the problem of obtaining the position and orientation, in the Cartesian space, of the end effector of the hand (hand s end) when the generalied coordinates are known is called the forward kinematics problem. This problem will be studied in Section 3. 7
3 Forward kinematics of position and orientation Ever point P in the three-dimensional Cartesian space can be represented b coordinates related to an reference sstem. Moreover, two reference sstems can relate to each other b means of a rotation of their coordinates aes and a translation of the coordinates origin. In Appendi A, those rotation matrices are defined and some properties are demonstrated. Those matrices will be used along the document. Therefore, if the coordinates of a point P related to one reference sstem S 1 are known, it is possible to obtain the coordinates related to another sstem, S 0, if the rotation R0 1 and translation matrices between both sstems are known: t 1 0 p 0 = R 1 0p 1 + t 1 0 (3.1) where p 0 represents the coordinates of point P related to S 0, p 1 represents the coordinates of the same point P related to S 1 and t 1 0 represents the translation vector defined in S 0. The transformation given b Equation 3.1, is called rigid motion. 6 6 5 5 q 6 4 3 3 q 5 4 q 4 2 q 3 1 2 1 q 2 O q 1 Figure 3.1: Local reference sstems. Figure 3.1 represents the local reference sstem S i of ever segment of the robot shown in Figure 2.1. Notice that onl two aes have been represented. The third one is obtained b the cross product of the other two: i = i i (3.2) As alread mentioned, the election of the reference sstem on ever segment is arbitrar. However, there eist sstematic representations as the Denavit-Hartenberg representation, which will not be treated in this document. Nevertheless, because all joints of the robot are rotational, in this document the local reference sstems will be defined as rotations of the kinematic pairs b keeping the name of the ais but modifing the subinde. Let l i be the distance between two joints of the kinematic pair (i 1, i) and (i, i + 1). Distance l 0 is defined as the distance between the anchor to the ground (point O) and the joint of the kinematic pair (0, 1). Therefore, the reference sstem S 0 is a motionless reference sstem parallel to the inertial reference sstem (,, ). If there is not an confusion, S 0 will be also used to represent 8
the inertial reference sstem. On the other hand, because the kinematic pair (n, n + 1) does not eist, a significant point Q in the n-th segment will be defined. This point Q is called the hand s end. This point is tpicall a tool when the robot is a manipulator, e.g. a gripper with two mobile fingers. In this eample, the point Q could be the geometric center between the gripping point of the fingers. Therefore, distance l n will be the distance between the kinematic pair (n 1, n) and the point Q. Moreover, let the set of rotation aes be defined as U = { u 1, u 2, u 3, u 4, u 5, u 6 }, where u i represents the rotation of the segment related to its kinematic pair. Therefore, for the eample of Figure 3.1 the set of rotation aes is: where 0 =. U = { 0, 1, 2, 3, 4, 5 } (3.3) 6 6 5 5 4 3 3 4 2 l 3 q 3 o 3 1 2 1 o 2 O Figure 3.2: Local reference sstems and kinematic pair (2, 3). The origin o i of ever local reference sstem S i is located at the joint of the kinematic pair (i, i+1). An eception eists with the origin o n that is located at the alread defined point Q. Figure 3.2 shows the kinematic pair (2, 3) that allows to define the generalied coordinate q 3. Moreover, according to the aforementioned notation, l i = o i 1 o i. Furthermore, let the vector d i i 1, declared in the reference sstem S i 1, be defined as: d i i 1 = o i 1 o i (3.4) Let the set Õ be defined as Õ = {õ 0, õ 1, õ 2, õ 3, õ 4, õ 5, õ 6 }, where õ i refers to the point o i declared in the reference sstem S i 1 prior to the rotation, that is for q i = 0. Therefore, for the robot of Figure 3.1 Õ = {l 0, l 1 0, l 2 1, l 3 2, l 4 3, l 5 4, l 6 5 } (3.5) According to this convention: d i i 1 = R i i 1õ i (3.6) 9
For the eample of the kinematic pair (2, 3) of Figure 3.2 d 3 2 = R 3 2õ 3 (3.7) Segment 3 rotates around u 3 = 2 (see Equation 3.3), õ 3 = l 3 2 (see Equation 3.5) and according to Appendi A: cos q 3 0 sin q 3 R2 3 = R,q3 = 0 1 0 (3.8) sin q 3 0 cos q 3 Therefore, according to Equation 3.6: cos q 3 0 sin q 3 3 cos q 3 d 3 2 = R2õ 3 3 = 0 1 0 l 0 = l 3 0 (3.9) sin q 3 0 cos q 3 0 sin q 3 where l 3 = o 2 o 3. In what follows, some recursive relations for vectors d j i for an point P in the space: are derived. According to Equation 3.1, p 0 = R0p i i + d i 0 (3.10a) p 0 = R0 i 1 p i 1 + d0 i 1 (3.10b) p i 1 = Ri 1p i i + d i i 1 (3.10c) where p 0 represents point P in coordinates of S 0, p i represents the same point P in coordinates of S i and p i 1 represents the same point P in coordinates of S i 1. Because the composition of rotations of local reference sstems satisfies the chain rule, that is: R i 0 = R 1 0R 2 1 R i i 1 (3.11) it will be satisfied, b substituting Equation 3.10c in 3.10b and subtracting Equation 3.10a, that d i 0 = d i 1 0 + R i 1 0 d i i 1 (3.12) Moreover, Equation 3.11 can be recursivel epressed as: R i 0 = R i 1 0 R i i 1 (3.13) Equation 3.12 represents the fundamental equation of the forward kinematics of position and Equation 3.13 represents the fundamental equation of the forward kinematics of orientation. Thanks to these two fundamental equations, the position of the final point of the hand Q = o n ( OQ = l0 k + o0 o n = l 0 k + d n 0 ) and its orientation R0 n can be calculated in the S 0 reference sstem. 10
4 Hand orientation vector (a, n, s) It is common to define the orientation of the hand b three orthonormal vectors {a, n, s}, that will be called approach, normal and sliding respectivel, where a = n = a a a n n n s s = s s (4.1a) (4.1b) (4.1c) The approach vector las in the direction of the gripper towards the object to grasp. The sliding vector describes the direction of the sliding of the fingers of the gripper and the normal vector is the third vector which completes the reference sstem of the hand. Figure 4.1 shows the orientation of the hand in the space. Q Q 1 Q 2 s n Q 1 Q 2 s Q a α a αs a Figure 4.1: Hand orientation and projection over plane XY. The orientation matri R SO referred to the inertial sstem, can be represented in different forms, depending on the definition of the approach vector. For eample, if the approach vector matches ais X, R SO will be represented b Equation 4.2a. On the other hand, if the approach vector matches ais Z, R SO will be represented b Equation 4.2b, a n s R SO = a n s (4.2a) a n s n s a R SO = n s a (4.2b) n s a Nonetheless, both cases match the following relationships: n = s a s = a n a = n s where represents the cross product and the dot product. (4.3a) (4.3b) (4.3c) a n = a s = n s = 0 (4.3d) 11
5 Kinematic decoupling Ever 6 DOFs mechanism, in which its last three joints intersect in a point, allows the kinematic decoupling of position and orientation. The point of intersection of the last three joints is called wrist center.this point is shown in Figure 5.1 as point C. This means that it is possible to solve the inverse kinematics problems of position and orientation independentl. More concretel, it is possible to obtain the generalied coordinates of the three first joints b solving an inverse kinematics problem of position and the last three ones b solving an inverse kinematics problem of orientation. The mechanism studied in the previous sections satisfies this condition, because it has an spheric wrist. Therefore, the movement of the last three joints around their rotation aes does not change the wrist center. Figure 5.1 shows this mechanism, where Q represents the position and {a, n, s} the orientation of the hand s end. l 3 l 2 l 4 C l 5 l 6 s l 1 Q l 0 O a C Q n Figure 5.1: Kinematic decoupling. The procedure must be implemented as follows: 1. Obtain the wrist center (C) when the approach vector ( a), the desired point of the hand s end (Q) and the hand dimension (d H = l 5 + l 6 ) are known: OC = OQ dh a (5.1) 2. Solve the inverse kinematics problem of position C to obtain {q 1, q 2, q 3 }. This will be studied in Section 6. 3. Calculate the rotation matri R 3 0 = R1 0 R2 1 R3 2 that will be called R S A. 4. Calculate R SH once the desired orientation of the mechanism R S0 and R SA are known, as: R SH = R T S A R S0 (5.2) 5. Solve the inverse kinematics problem of orientation R 6 3 to obtain {q 4, q 5, q 6 }. This will be studied in Section 7. 6 Inverse kinematics of position In this Section, a mechanism as the one shown in Figure 6.1, where C is the center of the wrist, is considered. This figure shows the arm of the robot. 12
3 l 4 3 C P l 3 q 3 2 1 2 l 2 q 2 l 1 1 l 0 q 1 P Figure 6.1: 4 DOFs mechanism. The inverse kinematics problem consists of obtaining the generalied coordinates of the center of the wrist when the coordinates of a point Q, fied in the hand s end, are known Q Q = Q (6.1) Q If the arm and hand can be decoupled, the approach ais can be located at a point C (wrist center) with coordinates C C = C (6.2) C The wrist center C can be calculated if the dimensions of the segments of the hand (l 5 and l 6 ) and the approach vector a are known, OC = OQ (l5 + l 6 ) a (6.3) where O is the coordinates origin of the reference sstem S O. The approach vector can be obtained from the rotation matri of the fied sstem at the hand s end (R SO ). For eample, if this matri has been defined as in Equation 4.2a, then A compact form of writing coordinates of point C is a n s 1 a = a n s 0 (6.4) a n s 0 OC = OQ (l5 + l 6 )R SO i (6.5) where 1 i = 0 (6.6) 0 13
If this matri was defined as in Equation 4.2b, that is, the approach ais was ais Z, OC = OQ (l5 + l 6 )R SO k (6.7) where 0 k = 0 (6.8) 1 The inverse kinematics problem of position consists of obtaining the generalied coordinates {q 1, q 2, q 3 } when the coordinates of point C are known. Figure 6.2 shows the two possible solutions for a similar eample. 2 2 1 1 l 1 l 2 l3 4 3 C l 4 4 3 2 l 1 1 3 l 2 l 3 l 4 3 1 C 4 4 l 0 l 0 2 (a) R I S A configuration. C (b) RS II A configuration. C Figure 6.2: Two different solutions to the same inverse kinematics problem of position. Appendi B shows how to obtain the different solutions {q 1, q 2, q 3 } when the arm rotation aes are { 0, 1, 2 }, where { atan2 (C, C ) C 0 C 0 q 1 = R C = C = 0 ( q 2 = β + atan2 L, ± ) (1 L 2 ) q 3 = q 2 + atan2 (C l 2 sin q 2, C l 2 cos q 2 ) β = atan2 (C, C ) L = l2 2 + C2 + C 2 (l 3 + l 4 ) 2 2l 2 (C 2 + C 2 ) C = C cos q 1 + C sin q 1 C = C (l 0 + l 1 ) (6.9a) (6.9b) (6.9c) (6.10a) (6.10b) (6.10c) (6.10d) 7 Inverse kinematics of orientation The inverse kinematics problem of orientation consists of obtaining the generalied coordinates when the rotation matri S O of a fi reference sstem at the hand s end is known. R SO = R 3 0R 6 3 (7.1) If the arm and hand can be decoupled, R SA = R0 3 is known if the inverse kinematics problem of position is alread solved. As a consequence, R SH = R3 6 can be obtained b solving: R SH = R T S A R SO (7.2) 14
3 4 3 5 s 4 5 6 6 O q 1 a C Q n Figure 7.1: R I S H configuration with R II S A. Figure 7.1 shows one of the possible solutions to the inverse kinematics problem of orientation for the solution R II S A of the inverse kinematics problem of position obtained in Section 6. 4 3 3 4 3 4 5 s 3 4 5 s 5 5 6 6 6 6 O q 1 a O q 1 a C Q n C Q n (a) R I S H configuration with R I S A (b) R I S H configuration with R II S A 3 3 3 4 s 3 4 s 4 5 6 6 5 4 5 5 6 6 O q 1 a O q 1 a C Q n C Q n (c) RS II H configuration with RS I A (d) RS II H configuration with RS II A Figure 7.2: Solutions to the inverse kinematics problem of orientation. Figure 7.2 shows the two possible solutions to the inverse kinematics problem of orientation for ever solution to the inverse kinematics problem of position obtained in Section 6. The rotation matri R3 6 is obtained as a composition of three successive rotations. Therefore, it has three unknowns variables which represent the generalied coordinates {q 4, q 5, q 6 } of the hand. That is: R 6 3 = R 4 3R 5 4R 6 5 (7.3) The generalied coordinates {q 4, q 5, q 6 } chosen represent the rotation angles relatives to the prior segment. Let the rotation aes be { 3, 4, 5 }. As a consequence: R 4 3 = R X,q4 R 5 4 = R Y,q5 R 6 5 = R X,q6 (7.4a) (7.4b) (7.4c) 15
where R X,q4, R Y,q5 and R X,q6 represent the elemental rotation matri around ais X, Y and X respectivel. 1 0 0 cos q 5 0 sin q 5 1 0 0 R3 6 = 0 cos q 4 sin q 4 0 1 0 0 cos q 6 sin q 6 (7.5) 0 sin q 4 cos q 4 sin q 5 0 cos q 5 0 sin q 6 cos q 6 B developing Equation 7.5 1, c 5 s 5 s 6 s 5 c 6 u 11 u 12 u 13 R3 6 = s 4 s 5 c 4 c 6 s 4 c 5 s 6 c 4 s 6 s 4 c 5 c 6 = U = u 21 u 22 u 23 (7.6) c 4 s 5 s 4 c 6 + c 4 c 5 s 6 s 4 s 6 + c 4 c 5 c 6 u 31 u 32 u 33 A specific case occurs when s 5 = 0. In this case, matri R3 6 remains as one of the following equations: 1 0 0 1 0 0 R3 6 = 0 c 4 c 6 s 4 s 6 c 4 s 6 s 4 c 6 = 0 cos (q 4 + q 6 ) sin (q 4 + q 6 ) (7.7a) 0 s 4 c 6 + c 4 s 6 s 4 s 6 + c 4 c 6 0 sin (q 4 + q 6 ) cos (q 4 + q 6 ) 1 0 0 1 0 0 R3 6 = 0 c 4 c 6 + s 4 s 6 c 4 s 6 + s 4 c 6 = 0 cos (q 4 q 6 ) sin (q 4 q 6 ) (7.7b) 0 s 4 c 6 c 4 s 6 s 4 s 6 c 4 c 6 0 sin (q 4 q 6 ) cos (q 4 q 6 ) Therefore, the inverse kinematics problem of orientation consists of solving the nine non-linear equations that can be obtained from identifing the following matrices: R 6 3 = R SH (7.8) However, because the are rotation matrices, it will onl be necessar to solve three independent equations of the identification. Therefore, solutions can be written as follows: ( ) q 5 = atan2 ± 1 u 2 11, u 11 atan2 (u 21, u 31 ), sin q 5 > 0 q 4 = atan2 ( u 21, u 31 ), sin q 5 < 0 u 11 q 6 + atan2 (u 32, u 22 ), u 11 = 1 atan2 (u 12, u 13 ), sin q 5 > 0 q 6 = atan2 ( u 12, u 13 ), sin q 5 < 0 u 11 ( q 4 + atan2 (u 32, u 22 )), u 11 = 1 (7.9a) (7.9b) (7.9c) It can be observed that two possible solutions are obtained when u 11 = 1. Moreover, infinite solutions can be found when u 11 = 1, because in this case there is a dependence between q 4 and q 6. 8 Kinematic conditions for q 4 = 0 This Section shows the conditions to be met when q 4 = 0, assuming that the arm and hand can be decoupled. To impose q 4 = 0 is equivalent to impose that the approach vector a belongs to the movement plane of the arm. 1 For clarit on the equations, sin and cos are replaced b s and c respectivell 16
3 4 3 C 4 s Q C q 1 n Q α a a a α Q Figure 8.1: Hand orientation with q 4 = 0. Figure 8.1 shows a situation in which q 4 = 0. R 6 3 given b Equation 7.6 when q 4 = 0 is converted to: c 5 s 5 s 6 s 5 c 6 R3 6 = 0 c 6 s 6 = U (8.1) s 5 c 5 s 6 c 5 c 6 It can be observed that it is immediate to obtain the generalied coordinates {q 5, q 6 }, q 5 = atan2 ( u 31, u 11 ) q 6 = atan2 ( u 23, u 22 ) (8.2a) (8.2b) Different from the general case where q 4 0, now there is a unique solution to the inverse kinematics problem of orientation. Moreover, the approach vector a will be limited to the plane defined b q 1. 17
A Vector operations and rotation matrices Definition A.1 (Orthogonal matri R). An squared orthogonal matri R is a matri which rows and columns form an orthonormal base of an Euclidean space, that is, their module is one and are orthogonal between them. The inverse of an orthogonal matri is its transpose matri, R T R = I (A.1) where I is the identit matri and R T is the transpose matri of R. The determinant of an orthogonal matri is det(r) = 1 or det(r) = 1, but not ever matri with a determinant 1 or 1 is orthogonal. Definition A.2 (Elemental rotation matrices R, R, R ). Rotation matrices R SO(n), where the set SO(n) is a special orthogonal group of dimension n. In general, the dimension of SO(n) is n(n 1). 2 The set SO(3) is of dimension 3, that means that it is alwas possible to transform a reference sstem into another one b a consecutive sequence of three rotations around a single ais (elementar rotations). The set SO(2) is of dimension 1, that means that it is alwas possible to transform a reference sstem into another one b a single rotation around a single ais. Let the elementar rotation matrices R SO(3) around ais, and be defined as: 1 0 0 R,θ = 0 cos θ sin θ 0 sin θ cos θ cos θ 0 sin θ R,θ = 0 1 0 sin θ 0 cos θ cos θ sin θ 0 R,θ = sin θ cos θ 0 0 0 1 The elementar rotation matri R SO(2) is defined as: [ ] cos θ sin θ R θ = sin θ cos θ (A.2a) (A.2b) (A.2c) (A.3) Proposition A.3 (Rotation composition). Let the matri R j i be defined as the rotation matri of the reference sstem S j with respect to S i. Given the rotation matrices R k i and R j k and the reference sstems S i, S j S k, it is true that R j i = Rk i R j k (A.4) For eample, R 3 0 can be rewritten b following the chain rule as: R 3 0 = R 1 0R 2 1R 3 2 (A.5) Proposition A.4 (Coordinates of a point in two reference sstems). Let point P be defined as a fied point in the space which coordinates in the reference sstem S i are given b vector p i and let matri R j i be defined as the rotation matri of the reference sstem S j with respect to S i. Then it is fulfilled that p i = R j i p j (A.6) For eample, if R j i is the elemental matri R θ SO(2), then if p j = (cos θ, sin θ) T, it is true that p i = (1, 0) T. It can be observed that p j = (R j i )T p i = R i j p i. 18
Proposition A.5 (Antismmetric matri S). Given two vectors a, b R 3, there is a single antismmetric matri S(a) such that a b = S(a)b (A.7) where represents the cross product and 0 a a S(a) = a 0 a a a 0 Antismmetric matrices S satisf the condition that S + S T = O. Proposition A.6. Because a b = b a it is fulfilled that (A.8) a b = S(b)a (A.9) Proposition A.7. For ever rotation matri R SO(3) R(a b) = (Ra) (Rb) = S(Ra)Rb = S(Rb)Ra (A.10) Proposition A.8. For ever rotation matri R SO(3) S(Ra)Rb = RS(a)b (A.11) Proposition A.9. B substituting c = Rb in A.11, for ever rotation matri R SO(3), it is true that RS(a)R T c = S(Ra)c (A.12) Proposition A.10. Because R T R = I, the square of the Euclidean norm of vector a can be rewritten as: a a = a T a = a T R T Ra = (Ra) T (Ra) = (Ra) (Ra) (A.13) where is the dot product. This means that the norm is invariant to rotations. In general, it can be assured that where R is an orthogonal matri. a b = (Ra) (Rb) (A.14) Proposition A.11. a b = tr(ab T ) = 1 2 tr[s(a)t S(b)] (A.15) where tr[a] represents the trace of matri A. The norm of Frobenius of matri A is defined as A F = tr[aa T ]. Proposition A.12. Given a rotation matri R(t) it is true that: Ṙ(t) = S(w(t))R(t) (A.16) where w(t) represents the angular speed vector and the antismmetric matri S(w(t)), defined as Equation A.8, is called angular speed tensor. Proposition A.12 demonstration. It is known that R T R = RR T = I. It can be said that Ṙ(t) = Ṙ(t)RT R It can be demonstrated that S = Ṙ(t)RT is an antismmetric matri. B deriving epression RR T = I, ṘR T + RṘT = O (A.17) (A.18) that is S + S T = O, hence S is an antismmetric matri. 19
B Solution to the inverse kinematics of position In this Section the equations of the inverse kinematics problem of position are solved b knowing the wrist s center C of the mechanism of Figure B.1. 3 l 4 3 C P l 3 q 3 2 1 2 l 2 q 2 l 1 1 l 0 q 1 P Figure B.1: 4 DOFs robot. Let the rotation aes of the robot be { 0, 1, 2, 3 }. Therefore, because 3 is the rotation ais of the fourth segment, the mechanism is equivalent, from the solution of the inverse kinematics problem of position perspective, to other mechanism of 3 DOFs in which the length of the last segment is l 3 + l 4. That means that the rotation around 3 does not affect to the solution of the inverse kinematics problem of position. Because the rotation ais of the first segment is 0, q 1 can be immediatel obtained as: q 1 = atan2 (C, C ) (B.1) To obtain q 2 and q 3 it is mandator to solve the following trigonometric equations sstem: C = l 2 cos q 2 + (l 3 + l 4 ) cos (q 2 + q 3 ) C = l 2 sin q 2 + (l 3 + l 4 ) sin (q 2 + q 3 ) (B.2a) (B.2b) where C = C cos q 1 C = C sin q 1 C = C + l 0 + l 1 (B.3a) (B.3b) (B.3c) (B.3d) The solution of C can be obtained from Equation B.3a and Equation B.3b: C = C cos q 1 + C sin q 1 (B.4) The equations sstem of Equation B.2 has two solutions that can be obtained b using: B simplifing Equation B.5: (C l 2 cos q 2 ) 2 + (C l 2 sin q 2 ) 2 = (l 3 + l 4 ) 2 (B.5) C cos q 2 + C sin q 2 = l2 2 + C2 + C 2 (l 3 + l 4 ) 2 2l 2 (B.6) 20
To solve Equation B.6, the epression of the sin of the angles sum can be used; therefore: sin (q 2 + β) = l2 2 + C2 + C 2 (l 3 + l 4 ) 2 2l 2 (C 2 + C 2 ) (B.7) where sin β = cos β = C (C 2 + C 2 ) C (C 2 + C 2 ) (B.8a) (B.8b) and in computational form: where ( q 2 = β + atan2 L, ± ) (1 L 2 ) β = atan2 (C, C ) L = l2 2 + C2 + C 2 (l 3 + l 4 ) 2 2l 2 (C 2 + C 2 ) (B.9) (B.10a) (B.10b) To obtain q 3 it is enough to solve the equations sstem given b Equation B.2, when q 2 is known; that is: q 3 = q 2 + atan2 (C l 2 sin q 2, C l 2 cos q 2 ) (B.11) As shown in Equation B.9, the inverse kinematics problem of position has two possible solutions, although there are singular conditions when C = C = 0, because Equation B.1 can not be applied. In this situation, q 1 could have an value. From Equation B.4 it can be obtained that if C = C = 0 then C = 0 and vice-versa. Moreover, if C = 0, then sin β = 0 so β = 0 or β = π obtaining two possible solutions for q 2 from Equation B.1, that is, sin q 2 = ±L. Nonetheless cos β = ±1, what implies that C = ±1. It can also be observed that if C = C = 0, β cannot be obtained from Equation B.10a. In this case, it is mandator that l 2 = l 3 + l 4, as can be checked from Equation B.5. This singularit can be easil solved from Equation B.2, as: 0 = l 2 cos q 2 + (l 3 + l 4 ) cos (q 2 + q 3 ) (B.12a) 0 = l 2 sin q 2 + (l 3 + l 4 ) sin (q 2 + q 3 ) (B.12b) B multipling Equation B.12a b sin q 2 and Equation B.12b b cos q 2 and subtracting one from the other, it can be obtained that sin q 3 = 0, so q 3 = 0 or q 3 = π. However, q 3 = 0 does not satisf Equations B.12, but q 3 = π does it for an value of q 2. However, when l 2 = l 3 + l 4 it is not necessar that C = C = 0. If C 0 or C 0 there is not this singularit and β could be obtained uniquel. Therefore, if the first singularit C = C = 0 is not met, neither is the second one. But if the first one is met and C = l 0 + l 1, the second singularit will be met. From the robot design point of view, it will be interesting that l 2 l 3 + l 4, and the singularit will be avoided. Finall, it can also occur that C = l 2 cos q 2 and C = l 2 sin q 2, so q 2 cannot be obtained form Equation B.9, because l 2 = (C 2 + C 2 ). Moreover, because Equation B.5, l3 + l 4 = 0 what is impossible ecept for l 3 = l 4 = 0. Therefore, this singularit cannot be produced because l 3 > 0, although it could be considered l 4 = 0 without loss of generaliation. This implies that it will alwas be possible to obtain q 3 given q 2 and using Equation B.12. In summar, if l 2 l 3 + l 4, there is onl the possibilit of the first singularit. 21
To conclude, and assuming that l 2 l 3 + l 4, the inverse kinematics problem of position has the following solutions: where { atan2 (C, C ) C 0 C 0 q 1 = R C = C = 0 ( q 2 = β + atan2 L, ± ) (1 L 2 ) q 3 = q 2 + atan2 (C l 2 sin q 2, C l 2 cos q 2 ) β = atan2 (C, C ) L = l2 2 + C2 + C 2 (l 3 + l 4 ) 2 2l 2 (C 2 + C 2 ) C = C cos q 1 + C sin q 1 C = C (l 0 + l 1 ) (B.13a) (B.13b) (B.13c) (B.14a) (B.14b) (B.14c) (B.14d) 22