ME 115(b): Solution to Homework #1

Transcription

1 ME 115(b): Solution to Homework #1 Solution to Problem #1: To construct the hybrid Jacobian for a manipulator, you could either construct the body Jacobian, JST b, and then use the body-to-hybrid velocity transformation: JST h RST 0 JST b 0 R ST or recall that the columns of the hybrid Jacobian take the form: θ N JST h (1) ω 1 ω 2 ω N where the forward kinematics equations g ST ( θ) take the form: g ST ( θ) RST ( θ) p ST ( θ) and ω j is: ω j This solution will use the second approach. ( RST RST T θ j ). Manipulator (ii): While this manipulator has a rather odd geometry, it is relatively straightforward to tackle this problem by suitable choices of geometry in the Denavit- Hartenburg approach. If β is the angle between the first and third joint axes when the manipulator lies in the configuration shown in the book, and if l 1 and l 2 are the two link lengths as shown in the book s figure, then the D-H parameters for this manipulator are: a 0 0 α 0 0 d 1 0 θ 1 variable a 1 0 α 1 π 2 d 2 0 θ 2 variable a 2 l 1 cos β α 2 π 2 d 3 (l 1 + l 2 ) sin β θ 3 variable a 3 l 2 cos β α 3 0 d 4 0 θ 4 0 where we have assumed that the tool frame z-axis is parallel to joint axis 3. Recalling the the relationship between link frames in terms of the D-H parameters: cos θ i+1 sin θ i+1 0 a i g i,i+1 sin θ i+1 cos α i cos θ i+1 cos α i sin α i d i+1 sin α i sin θ i+1 sin α i cos θ i+1 sin α i cos α i d i+1 cos α i

2 The forward kinematics of this mechanism can be found as: g ST g S,1 g 1,2 g 2,3 g 3,T c 1 s c 2 s c 3 s 3 0 a a 3 s 1 c d s 2 c s 3 c (2) (c 1 c 2 c 3 s 1 s 3 ) (c 1 c 2 s 3 + s 1 c 3 ) c 1 s 2 (a 3 c 1 c 2 c 3 + a 2 c 1 c 2 + d 3 c 1 s 2 a 3 s 1 s 3 ) (s 1 c 2 c 3 + c 1 s 3 ) ( s 1 c 2 c 3 + c 1 c 3 ) s 1 s 2 (a 3 s 1 c 2 c 3 + a 2 s 1 c 2 + d 3 s 1 s 2 + a 3 c 1 s 3 ) s 2 c 3 s 2 s 3 c 2 ( a 3 s 2 c 3 a 2 s 2 + d 3 c 2 ) Following Equation (1), the hybrid Jacobian is: J h ST ( v1 v 2 v 3 ω 1 ω 2 ω 3 θ 3 RST T ) (3) where: ( a 3 s 1 c 2 c 3 a 2 s 1 c 2 d 3 s 1 s 2 a 3 c 1 s 3 ) v 1 (a 3 c 1 c 2 c 3 + a 2 c 1 c 2 + d 3 c 1 s 2 a 3 s 1 s 3 ) 0 ( a 3 c 1 s 2 c 3 a 2 c 1 s 2 + d 3 c 1 c 2 ) v 2 ( a 3 s 1 s 2 c 3 a 2 s 1 s 2 + d 3 s 1 c 2 ) ( a 3 c 1 c 2 s 3 a 3 s 1 c 3 ) v 3 ( a 2 s 1 c 2 s 3 + a 3 c 1 c 3 ) ( a 3 c 2 c 3 a 2 c 2 d 3 s 2 ) (a 3 s 2 s 3 ) 0 s 1 c 1 s 2 ω1 ω 2 ω 3 0 c 1 s 1 s c 2 Manipulator (iv): This is the Stanford Manipulator. Recall from a previous homework solution that the Denavit-Hartenberg parameters and the forward kinematics are: a 0 0 α 0 0 d 1 0 θ 1 variable a 1 0 α 1 π 2 d 2 0 θ 2 variable a 2 0 α 2 π 2 d 3 variable θ 3 0 (constant) 2

3 g ST g S,1 g 1,2 g 2,3 g 3,T (4) c 1 s c 2 s s 1 c d s 2 c (5) c 1 c 2 s 1 c 1 s 2 d 3 c 1 s 2 s 1 c 2 c 1 s 1 s 2 d 3 s 1 s 2 s 2 0 c 2 d 3 c 2 RST (θ 1, θ 2, d 3 ) p ST (θ 1, θ 2, d 3 ) 0 T (6) Following Equation (1), the hybrid Jacobian is: J h ST ( RST T d 3 s 1 s 2 d 3 c 1 c 2 c 1 s 2 d 3 c 2 s 2 d 3 s 1 c 2 s 1 s 2 0 d 3 s 2 c 2 0 s c d 3 ) (7) (8) (9) Solution to Problem #2: To find the singularities of the regional part (just the first three joints) of the elbow manipulator, one can determine the conditions under which the Jacobian matrix of the manipulator loses rank. While one could use any Jacobian, for simplicity we will use the Hybrid Jacobian matrix. You could either recall from the class note (or derive) the forward kinematics of the Elbow manipulator: g ST ( θ) RST ( θ) p ST ( θ) c 1 c 23 s 1 c 1 s 23 c 1 (l 2 c 2 + l 3 c 23 ) s 1 c 23 c 1 s 1 s 23 s 1 (l 2 c 2 + l 3 c 23 ) s 23 0 c 23 (l 2 s 2 + l 3 s 23 ) (10) where c j cos(θ j ), s j sin(θ j ), c ij cos(θ i + θ j ), s ij sin(θ i + θ j ), etc. Recall that the hybrid Jacobian for the regional part of a manipulator is defined as: JST h (11) 3 pst θ

4 and thus substituting Equation (10) into Equation (11) yields: s 1 (l 2 c 2 + l 3 c 23 ) c 1 (l 2 s 2 + l 3 s 23 ) l 3 c 1 s 23 JST h c 1 (l 2 c 2 + l 3 c 23 ) s 1 (l 2 s 2 + l 3 s 23 ) l 3 s 1 s 23. (12) 0 (l 2 c 2 + l 3 c 23 ) l 3 c 23 Singularities will occur when the determinant of J h ST is zero: The singularities occur when: det(j h ST ) l 2 l 3 l 2 cos(θ 2 ) + l 3 cos(θ 2 + θ 3 ) sin(θ 3 ) θ 3 0. In this case, the arm is fully streteched out, and thus this singular configuration corresponds to the manipulator s outer workspace limit. θ 3 ±π. In this case, the arm is folded back on itself, and this singular configuration corresponds to the manipulator s inner workspace boundary. l 2 c 2 + l 3 c Note from the forward kinematics equations that in this case, x and y coordinates of the tool frame origin lie at x 0 and y 0. This occurs when the tool frame origin is placed anywhere along the first joint axis. Solution to Problem #3: Part (a): From Problem 1 above we know that regional forward kinematics that relates the joint variables (θ 1, θ 2, d 3 ) of the Stanford manipulator to the Cartesian location of the tool frame origin is: x d 3 c 1 s 2 p ST y d 3 s 1 s 2 (13) z d 3 c 2 T x where p ST y is the location of the tool frame origin. The inverse kinematic solution z can be derived by simple algebraic manipulation of these kinematic relationships. 1. Step #1: x 2 + y 2 + z 2 d 2 3 d 3 ± x 2 + y 2 + z 2. There are two solutions to this equation. In practice, one of these solutions will almost certainly be infeasible from a mechanical point of view. 2. Step #2: x 2 + y 2 d 2 3s 2 x 2 s 2 ± 2 +y 2. There are 4 possible solutions to x 2 +y 2 +z 2 this equation. For each different of the two possible d 3 solutions there are two different θ 2 solutions. 4

5 3. Step #3: From the forward kinematics, we have that: cos θ 1 x d 3 sin θ 2 ; sin θ 1 from which we can get the unique solution: θ 1 There is a single θ 1 solution for each (θ 2, d 3 ) combination. y d 3 sin θ 2 ; (14) y x Atan2,. (15) d 3 sin θ 2 d 3 sin θ 2 Part (b): This problem is most readily solved by realizing that manipulators with wrists have a unique forward kinematic (and therefore inverse kinematic) solution structure. Note that a wrist is defined as a serial kinematic chain of three revolute joints whose joint axes all intersect at a single point, which we will term the wrist center. The wrist center will also be the origin of the references frames (as defined by the Denavit-Hartenberg convention) of those links that make up the wrist. Let the form of the forward kinematic relationship be denoted by: g ST RST (θ 1,..., θ 6 ) p ST (θ 1,..., θ 6 ) R D ST p D ST where RST D and pd ST are the desired orientation and position of the tool frame. Let g 6,T denote the location of the tool frame relative to the Denavit-Hartenberg reference frame of link 6 (whose origin will lie at the wrist point). g 6,T is a constant transformation that accounts for any difference in the location of the tool relative to the link frame. The desired position and orientation of link frame 6 will thus be: g D S,6 R D ST p D ST R6,T p 6,T 1 R D S,6 p D S,6 (16). (17) Note that the forward kinematics formula which describes the origin of Link frame 6 (the wrist center point) is only a function of (θ 1, θ 2, d 3 ), and is in fact the formula used in part (a) of this problem. Thus, (θ 1, θ 2, d 3 ) can be found from part (a), using p D S,6 is the desired position of the wrist center point. Note that the forward kinematics relationship that describes the orientation of link frame 6 is: R D S,6 R D S,T R 1 6,T R S,1(θ 1 ) R 1,2 (θ 2 ) R 2,3 R 3,4 (θ 4 ) R 4,5 (θ 5 ) R 5,6 (θ 6 ). (18) But (θ 1, θ 2, d 3 ) have already been determined by the placement of the wrist point at its desired location. Hence, we can rearrange Equation (18) to isolate the unknown joint variables (θ 4, θ 5, θ 6 ): R 3,4 (θ 4 ) R 4,5 (θ 5 ) R 5,6 (θ 6 ) (R S,1 R 1,2 R 2,3 ) 1 RS,T D R 1 6,T. (19) All of the terms on the right hand side of Equation (19) are known from the inverse kinematic problem state, the knowledge of g 6,T, and the inverse kinematic solution of the regional structure. 5

6 Note that the general form of the rotation matrix using the Denavit-Hartenberg convention is: cos θ i sin θ i 0 R i 1,i sin θ i cos α i 1 cos θ i cos α i 1 sin α i 1 (20) sin θ i sin α i 1 cos θ i sin α i 1 cos α i 1 For the Stanford manipulator, one choice 1 of the twist angles for the wrist substructure is: α 3 π/2, α 4 π/2, α 5 π/2. Hence: c 4 s 4 0 c 5 s 5 0 c 6 s 6 0 R 3, R 4, R 5, (21) s 4 c 4 0 s 5 c 5 0 s 6 c 6 0 and therefore: (c 4 c 5 c 6 s 4 s 6 ) (c 4 c 5 s 6 + s 4 c 6 ) c 4 s 5 R 3,4 R 4,5 R 5,6 s 5 c 6 s 5 s 6 c 5 (22) (s 4 c 5 c 6 + c 4 s 6 ) (s 4 c 5 s 6 c 4 c 6 ) s 4 s 5 Let the entries constant matrix R3,6 D (R S,1 R 1,2 R 2,3 ) 1 RS,T D R 1 6,T r 11 r 12 r 13 R3,6 D r 21 r 22 r 23. r 31 r 32 r 33 Equating terms, we can see that: cos θ 5 r 23 which has two solutions. Using these solutions, we see that: r 33 θ 4 Atan2, r 13 sin θ 5 sin θ 5 θ 6 Atan2 r 22 r 21, sin θ 5 sin θ 5 have the form: Hence, the wrist has two independent solutions, for each of the four different solutions to the regional structure inverse kinematics, yielding 8 different inverse kinematic solutions. angles 1 any other choice of twist angles that satisfy the D-H convention will differ only in the signs of these 6