Kinematics of a UR5. Rasmus Skovgaard Andersen Aalborg University

Size: px

Start display at page:

Download "Kinematics of a UR5. Rasmus Skovgaard Andersen Aalborg University"

Jade Blake
5 years ago
Views:

1 Kinematics of a UR5 May 3, 28 Rasmus Skovgaard Andersen Aalborg University Contents Introduction Notation Forward Kinematics for UR Inverse Kinematics for UR Finding θ Finding θ Finding θ Finding θ Finding θ Finding θ Discussion Additional Material References Introduction This worksheet describes how to derive the forward and inverse kinematic equations of a UR5 robot. The worksheet is inspired by [Hawkins, 23], [Keating, 27], and [Kebria et al., 26] but attempts to explain each step more thoroughly.. Notation The worksheet follows the Denavit-Hartenberg notation used by [Craig, 25], sometimes referred to as modified DH-parameters. Additionally, the following brief notations are used: P 6 = P 6x P 6y P 6z is the origin of frame 6 seen from frame. Ŷ 6 = Ŷ 6x Ŷ 6y Ŷ 6z is a unit vector giving the direction of the y-axis of frame 6 seen from frame.

2 6 T is a transformation from frame 6 to frame, meaning that P = 6 T 6P. 2 Forward Kinematics for UR5 The forward kinematic (FK) equations calculates a transformation matrix 6T based on known joint angles θ 6. The transformation matrix is defined as: [ ] 6 6 T (θ R P 6, θ 2, θ 3, θ 4, θ 5, θ 6 ) = [ ˆX6 Ŷ Ẑ ] = 6 6 P 6 ˆX6x Ŷ Ẑ 6x 6x P 6x ˆX6y Ŷ Ẑ = 6y 6y P 6y ˆX6z Ŷ Ẑ () 6z 6z P 6z, where the columns ˆX6, Ŷ 6, and Ẑ 6 are unit vectors defining the axes of frame 6 in relation to frame. We can split the transformation matrix is given as a chain of transformations; one for each joint: 6 T (θ, θ 2, θ 3, θ 4, θ 5, θ 6 ) = T (θ ) 2 T (θ 2) 2 3 T (θ 3) 3 4 T (θ 4) 4 5 T (θ 5) 5 6 T (θ 6) (2) The kinematic structure of the UR5 robot in zero position is shown in Figure : y 2 z a 2 x,2 z 2 a 3 d 4 shoulder y 3 y 4 y 3 z 3 z 4 y 6 z 6 y d z x 3 elbow x 4 x 6 tool x d 5 y 5 y base x 5 wrist d 6 z 5 (a) Frames describing the UR5 robot. (b) Visualization Figure : The UR5 robot in zero position. 2

3 The DH-parameters are according to [Craig, 25] specified as: a i = distance from Z i to Z i+ measured along X i α i = angle from Z i to Z i+ measured about X i d i = distance from X i to X i measured along Z i θ i = angle from X i to X i measured about Z i (3) For the UR5, the DH parameters are: i α i a i d i θ i d θ 2 α = 9 θ 2 3 a 2 θ 3 4 a 3 d 4 θ 4 5 α 4 = 9 d 5 θ 5 6 α 5 = 9 d 6 θ 6 Table : Modified Denavit-Harteberg parameters (DH-parameters) of a UR5 robot, corresponding to the frames in Figure. The parameters θ i are variables and the remaining parameters are constants. The DH-parameters can be used to write the transformations for each link. general transformation between link i and i is given by : cos θ i sin θ i a i i- i T = sin θ i cos(α i ) cos θ i cos(α i ) sin(α i ) sin(α i )d i sin θ i sin(α i ) cos θ i sin(α i ) cos(α i ) cos(α i )d i The (4) It is straightforward to write the transformation matrix for each link of the UR5 robot by inserting the DH-parameters from Table in Equation (4). The complete transformation from base to end-effector can then be derived by multiplication of all 6 transformation matrices, as shown in Equation (2). expressions for all 2 parameters in the transformation matrix 6 T. analytic equations can be found in [Hawkins, 23]. The result is analytical The complete 3 Inverse Kinematics for UR5 The inverse kinematic (IK) equations calculates the joint angles θ 6 based on a desired position and orientation of the final frame, specified as the transformation 6 T. In the solution, we restrict all angles as (θ,..., θ 6 ) [; 2π[. Equation 3.6 in [Craig, 25] 3

4 3. Finding θ To find θ, we first determine the location of frame 5 (the wrist frame) in relation to the base frame; P 5. As illustrated in Figure 2, P 5 can be found by translating backwards from frame 6 to frame 5 along z 6. Remember that both 6 T and d 6 are known. x 6 y 6 z6 x z T 6 y 5 x 5 d 6 y P 5 z 5 Figure 2: Finding the origin of frame 5. The translation P 5 can be written as: P 5 = P 6 d 6 Ẑ 6 P 5 = 6 T d 6 (5) To derive θ, we examine the robot seen from above in Figure 3 (looking down into z ): 4

5 y P 5 x x P 5 y x φ z 2 θ x θ x z 3 φ 2 y P 5 xy y P 5 xy d 4 z 4 z 5 (a) (b) Figure 3: Robot (until frame 5) seen from above. The robot is shown as grey lines. Note that z 5 should actually point into the page if all angles are. Here θ 4 in order to better illustrate how θ can be isolated. Our approach to finding θ is to consider the wrist, P 5, seen from frame and frame, respectively. Intuitively, the rotation from frame -to-, θ, should equal the difference between the rotations from -to-5 and -to-5. Formalizing and inserting symbols from Figure 3 give: v = v 5 v 5 v = v 5 + v 5 ( θ = φ + φ 2 + π ) 2 (6) The angle φ can be found by examining the triangle with sides P 5x and P 5y : φ = atan2( P 5y, P 5x ) (7) The angle φ 2 is found by examining the rightmost triangle with φ 2 as one of the angles. Two of the sides have lengths P 5xy and d 4 : cos(φ 2 ) = d 4 P 5xy ( d 4 φ 2 = ±acos P 5xy ) d 4 φ 2 = ±acos (8) 2 P 5x + 2 P 5y 5

y y x θ x θ P 6 y z 3 y P 6 y y x 4 θ 5 d 4 d 6 cos(θ 5 ) z 5 z 4 x 5 P 6 y x 4 θ 5 x 5 P 6 y P 6 z 6 P 6 x P P 6 x (a) (b) Figure 4: Robot (including frame 6) seen from above.

6 The desired angle θ can now be found simply as: θ = φ + φ 2 + π 2 ( ) θ = atan2 P 5y, P 5x ± acos d 4 P 5x 2 + P 5y 2 + π 2 (9) The two solutions correspond to the shoulder being left or right. 3.2 Finding θ 5 Figure 4 shows the robot from above again; this time with frame 6 included. y y x θ x θ P 6 y z 3 y P 6 y y x 4 θ 5 d 4 d 6 cos(θ 5 ) z 5 z 4 x 5 P 6 y x 4 θ 5 x 5 P 6 y P 6 z 6 P 6 x P P 6 x (a) (b) Figure 4: Robot (including frame 6) seen from above. Our approach to finding θ 5 is to notice that P 6y (the y-component of P 6 ) only depends on θ 5. In the figure, we can trace y backwards to see that P 6y is given by: P 6y = d 4 + d 6 cos θ 5 () The component P 6y can also be expressed by looking at P 6 as a rotation of P 6 6

7 around z : P 6 = R P 6 P 6 = R P 6 cos(θ ) sin(θ ) = sin(θ ) cos(θ ) P 6x P 6y P 6z P 6x P 6y P 6z cos(θ ) sin(θ ) = sin(θ ) cos(θ ) P 6x P 6y P 6z P 6x P 6y P 6z P 6y = P 6x ( sin θ ) + P 6y cos θ () By combining Equation () and (), we can eliminate P 6y and express θ 5 only using known values: d 4 d 6 cos θ 5 = P 6x ( sin θ ) + P 6y cos θ P 6x sin θ P 6y cos θ d 4 cos θ 5 = d ( 6 ) P 6x sin θ P 6y cos θ d 4 θ 5 = ±acos d 6 (2) Again there are two solutions. These correspond to the wrist being up or down, respectively. This can be interpreted intuitively: The joint sum (θ 2 + θ 3 + θ 4 ) can cause the end-effector to be located in the same position, but with the wrist flipped. The orientation can then be corrected by θ 6. Also note that a solution is defined as long as the value inside acos has a magnitude not greater than ; equivalent to P 6y d 4 d Finding θ 6 To determine θ 6 we examine y seen from frame 6; 6 Ŷ. This axis will (ignoring translations) always be parallel to 6 Ẑ 2,3,4, as can be seen from Figure 5b. Therefore, it will only depend on θ 5 and θ 6. It turns out that 6 Ŷ can in fact be described using spherical coordinates, where azimuth is θ 6 and the polar angle is θ 5 ; see Figure 5a. 7

8 z 6 θ 5 z z 2 z 3 y 4 x x 4 z 4 y 6 z 6 y =z 4 y x 6 θ 6 y 6 y 5 x 6 θ 6 x 5 z 5 θ 5 (a) Polar angles for 6 Ŷ (b) Reference view of the relevant frames Figure 5: The axis 6 Ŷ expressed in spherical coordinates with azimuth θ 6 and polar angle θ 5. For simplicity, 6 Ŷ is denoted y in the Figure. Converting 6 Ŷ from spherical to Cartesian coordinates gives: sin θ 5 cos( θ 6 ) 6 Ŷ = sin θ 5 sin( θ 6 ) cos θ 5 6Ŷ sin θ 5 cos θ 6 = sin θ 5 sin θ 6 (3) cos θ 5 In Equation (3) we could isolate θ 6 and have an expression of θ 6 in relation to 6 T. We want an expression of θ 6 all the way from 6 T. To get this, we identify that 6 Ŷ is given as a rotation of θ in the x, y-plane of frame (very similar to Equation ()): 6Ŷ = 6 ˆX ( sin θ ) + 6 Ŷ cos θ 6Ŷ 6 ˆXx sin θ + 6 Ŷ x cos θ = 6 ˆXy sin θ + 6 Ŷ y cos θ (4) 6 ˆXz sin θ + 6 Ŷ z cos θ Equating the first two entries of (3) and (4) give: sin θ 5 cos θ 6 = 6 ˆXx sin θ + 6 } Ŷ x cos θ sin θ 5 sin θ 6 = 6 ˆXy sin θ + 6 (5) Ŷ y cos θ cos θ 6 = 6 ˆXx sin θ 6 Ŷ x cos θ sin θ 5 sin θ 6 = 6 ˆXy sin θ + 6 Ŷ y cos θ sin θ 5 ( 6 ˆXy sin θ + 6 Ŷ y cos θ 6 ˆXx sin θ 6 ) Ŷ x cos θ θ 6 = atan2, (6) sin θ 5 sin θ 5 This solution is undetermined if the denominator sin θ 5 =. In this case, the joint axes 2, 3, 4 and 6 are aligned (as in Figure 5b). This is too many degrees of freedom. 8

9 The axes 2, 3, and 4 can on their own rotate the end-effector (frame 6) around z 6 without moving it, and the 6 th joint therefore becomes redundant. In this case, θ 6 can simply be set to an arbitrary value. If both of the numerators in Equation (6) are, the solution is also undetermined. If this is the case, sin θ 5 must also be, and the situation is thus the same. This can be seen by examining both sides in Equation (5). 3.4 Finding θ 3 We examine the remaining three joints (2, 3, and 4). Notice that their joint axes are all parallel. Together they constitute a planar 3R-manipulator, as illustrated in Figure 6. x x 2 θ 2 z y 2 φ φ 2 θ 2 a 2 x 3 P 4 x y 3 elbow P 4 z x 2 z y 2 P 4 x x x 3 P 4 z P 4 xz φ 3 θ 3 a 3 P 4 xz x 4 x 4 y 4 (a) (b) Figure 6: Joint 2, 3, and 4 together constitutes a 3R planar manipulator. We can constrict ourselves to look at 4T (frame 4 in relation to frame ) because T, 4 5 T, and 5 6T at this point are known. This transformation is illustrated in the x, z-plane of frame in Figure 6a. From the figure it is clear that the length of the translation P 4xz is determined only by θ 3, or similarly by φ 3. The angle φ 3 can be found by using the law of cosine: cos φ 3 = ( a 2) 2 + ( a 3 ) 2 P 4xz 2 2( a 2 )( a 3 ) = a a 3 2 P 4xz 2 2a 2 a 3 (7) The relationship between cos φ 3 and cos θ 3 is: cos θ 3 = cos(π φ 3 ) = cos(φ 3 ) (8) 9

10 Combining (7) and (8) give: cos θ 3 = a a 2 3 P 4xz 2 2a 2 a 3 ( P 4xz 2 a 2 ) 2 a 2 3 θ 3 = ±acos 2a 2 a 3 (9) Note that solutions exist for θ 3 if the argument of acos is within [ ; ]. It can be shown that this is equivalent to P 4xz [ a 2 a 3 ; a 2 + a 3 ]. In most cases where solutions exist, there will exist two different solutions. These correspond to elbow up and elbow down. 3.5 Finding θ 2 The angle θ 2 can be found as φ φ 2. Each of these can be found by inspecting Figure 6a and using atan2 and sine relations: φ = atan2( P 4z, P 4x ) (2) sin φ 2 a 3 = sin φ 3 φ 2 = asin P 4xz ( a3 sin φ 3 P 4xz ) (2) We can replace φ 3 with θ 3 by noticing that sin φ 3 = sin (8 θ 3 ) = sin θ 3. Combining the equations now give: θ 2 = φ φ 2 = atan2( P 4z, P 4x ) asin ( ) a3 sin θ 3 P 4xz (22) 3.6 Finding θ 4 The last remaining angle θ 4 is defined as the angle from X 3 to X 4 measured about Z 4 (c.f. Equation (3) in page 3). It can thus easily be derived from the last remaining transformation matrix, 3 4 T, using its first column 3 ˆX4 : θ 4 = atan2( 3 ˆX4y, 3 ˆX4x ) (23) 4 Discussion To sum up, a total of 8 solutions exist in general for the general inverse kinematic problem of the UR5: 2 θ 2 θ5 θ6 2 θ3 θ2 θ4. 4. Additional Material Each of the sources used as inspiration for this document contain more information in specific subjects. Most notably: [Hawkins, 23], [Keating, 27], and [Kebria et al., 26] Full FK solution is included in [Hawkins, 23].

11 Dynamics is briefly covered in [Kebria et al., 26]. An extra axis is added in [Hawkins, 23]. Multiple IK solutions are visualized in [Keating, 27]

12 5 References [Craig, 25] Craig, J. J. (25). Introduction to robotics: mechanics and control, volume 3. Pearson/Prentice Hall Upper Saddle River, NJ, USA:. [Hawkins, 23] Hawkins, K. P. (23). Analytic inverse kinematics for the universal robots ur-5/ur- arms. Technical report, Georgia Institute of Technology. Available at: kin_tech_report_.pdf. [Keating, 27] Keating, R. (27). Analytic inverse kinematics for the universal robots ur-5/ur- arms. Technical report, John Hopkins. Available at: [Kebria et al., 26] Kebria, P. M., Al-Wais, S., Abdi, H., and Nahavandi, S. (26). Kinematic and dynamic modelling of ur5 manipulator. In Systems, Man, and Cybernetics (SMC), 26 IEEE International Conference on, pages IEEE. 2

Lecture Note 8: Inverse Kinematics

ECE5463: Introduction to Robotics Lecture Note 8: Inverse Kinematics Prof. Wei Zhang Department of Electrical and Computer Engineering Ohio State University Columbus, Ohio, USA Spring 2018 Lecture 8 (ECE5463