DIFFERENTIAL KINEMATICS. Geometric Jacobian. Analytical Jacobian. Kinematic singularities. Kinematic redundancy. Inverse differential kinematics

Transcription

1 DIFFERENTIAL KINEMATICS relationship between joint velocities and end-effector velocities Geometric Jacobian Analytical Jacobian Kinematic singularities Kinematic redundancy Inverse differential kinematics Inverse kinematics algorithms STATICS relationship between end-effector forces and joint torques

2 GEOMETRIC JACOBIAN T(q) = R(q) p(q) 0 T 1 Goal ṗ = J P (q) q ω = J O (q) q v = [ ] ṗ ω = J(q) q

3 Derivative of a rotation matrix R(t)R T (t) = I Ṙ(t)R T (t) + R(t)ṘT (t) = O Given S(t) = Ṙ(t)RT (t) S(t) + S T (t) = O Ṙ(t) = S(ω(t))R(t) S = 0 ω z ω y ω z 0 ω x ω y ω x 0

4 Example R z (α) = cosα sinα 0 sinα cosα S(t) = = αsinα α cosα 0 cosα sinα 0 α cosα α sinα 0 sinα cosα α 0 α 0 0 = S(ω(t)) 0 0 0

5 p 0 = o R 0 1p 1 ṗ 0 = ȯ R0 1ṗ1 + Ṙ0 1 p1 = ȯ R 0 1ṗ1 + S(ω 0 1)R 0 1p 1 = ȯ R 0 1ṗ1 + ω 0 1 r 0 1

6 Link velocity Linear velocity p i = p i 1 + R i 1 r i 1 i 1,i ṗ i = ṗ i 1 + R i 1 ṙ i 1 i 1,i + ω i 1 R i 1 r i 1 i 1,i = ṗ i 1 + v i 1,i + ω i 1 r i 1,i

7 Angular velocity R i = R i 1 R i 1 i S(ω i )R i = S(ω i 1 )R i + R i 1 S(ω i 1 i 1,i )Ri 1 i = S(ω i 1 )R i + S(R i 1 ω i 1 i 1,i )R i ω i = ω i 1 + R i 1 ω i 1 i 1,i = ω i 1 + ω i 1,i

8 ω i = ω i 1 + ω i 1,i ṗ i = ṗ i 1 + v i 1,i + ω i 1 r i 1,i Prismatic joint ω i 1,i = 0 v i 1,i = d i z i 1 ω i = ω i 1 ṗ i = ṗ i 1 + d i z i 1 + ω i r i 1,i Revolute joint ω i 1,i = ϑ i z i 1 v i 1,i = ω i 1,i r i 1,i ω i = ω i 1 + ϑ i z i 1 ṗ i = ṗ i 1 + ω i r i 1,i

9 Jacobian computation J = j P1... j Pn j O1 j On Angular velocity Joint i prismatic q i j Oi = 0 = j Oi = 0 Joint i revolute q i j Oi = ϑ i z i 1 = j Oi = z i 1

10 Linear velocity Joint i prismatic q i j Pi = d i z i 1 = j Pi = z i 1 Joint i revolute q i j Pi = ω i 1,i r i 1,n = ϑ i z i 1 (p p i 1 ) j Pi = z i 1 (p p i 1 )

11 Column of geometric Jacobian [ jpi j Oi ] [ ] zi 1 0 = [ ] zi 1 (p p i 1 ) z i 1 prismatic joint revolute joint z i 1 = R 0 1(q 1 )...R i 2 i 1 (q i 1)z 0 p = A 0 1 (q 1)...A n 1 n (q n ) p 0 p i 1 = A 0 1(q 1 )...A i 2 i 1 (q i 1) p 0

12 Representation in different frame [ ] ṗt ω t = = J t = [ ] [ ] R t O ṗ O R t ω [ ] R t O O R t J q [ R t O O R t ] J

13 Three link planar aram

14 J(q) = [ ] z0 (p p 0 ) z 1 (p p 1 ) z 2 (p p 2 ) z 0 z 1 z 2 p 0 = p 1 = a 1c 1 a 1 s 1 0 p 2 = a 1c 1 + a 2 c 12 a 1 s 1 + a 2 s 12 0 p = a 1c 1 + a 2 c 12 + a 3 c 123 a 1 s 1 + a 2 s 12 + a 3 s z 0 = z 1 = z 2 = 0 0 1

15 J = a 1 s 1 a 2 s 12 a 3 s 123 a 2 s 12 a 3 s 123 a 3 s 123 a 1 c 1 + a 2 c 12 + a 3 c 123 a 2 c 12 + a 3 c 123 a 3 c J P = [ ] a1 s 1 a 2 s 12 a 3 s 123 a 2 s 12 a 3 s 123 a 3 s 123 a 1 c 1 + a 2 c 12 + a 3 c 123 a 2 c 12 + a 3 c 123 a 3 c 123

16 Anthropomorphic arm

17 J = [ ] z0 (p p 0 ) z 1 (p p 1 ) z 2 (p p 2 ) z 0 z 1 z 2 p 0 = p 1 = 0 0 p 2 = 0 z 0 = p = c 1(a 2 c 2 + a 3 c 23 ) s 1 (a 2 c 2 + a 3 c 23 ) a 2 s 2 + a 3 s 23 z 1 = z 2 = a 2c 1 c 2 a 2 s 1 c 2 a 2 s 2 s 1 c 1 0

18 J = s 1 (a 2 c 2 + a 3 c 23 ) c 1 (a 2 s 2 + a 3 s 23 ) a 3 c 1 s 23 c 1 (a 2 c 2 + a 3 c 23 ) s 1 (a 2 s 2 + a 3 s 23 ) a 3 s 1 s 23 0 a 2 c 2 + a 3 c 23 a 3 c 23 0 s 1 s 1 0 c 1 c J P = s 1(a 2 c 2 + a 3 c 23 ) c 1 (a 2 s 2 + a 3 s 23 ) a 3 c 1 s 23 c 1 (a 2 c 2 + a 3 c 23 ) s 1 (a 2 s 2 + a 3 s 23 ) a 3 s 1 s 23 0 a 2 c 2 + a 3 c 23 a 3 c 23

19 Stanford manipulator

20 J = [ z0 (p p 0 ) z 1 (p p 1 ) z 2 z 0 z 1 0 z 3 (p p 3 ) z 4 (p p 4 ) z 5 (p p 5 ) z 3 z 4 z 5 ] p 0 = p 1 = p 3 = p 4 = p 5 = c 1s 2 d 3 s 1 d 2 s 1 s 2 d 3 + c 1 d 2 c 2 d 3 p = c 1s 2 d 3 s 1 d 2 + d 6 (c 1 c 2 c 4 s 5 + c 1 c 5 s 2 s 1 s 4 s 5 ) s 1 s 2 d 3 + c 1 d 2 + d 6 (c 1 s 4 s 5 + c 2 c 4 s 1 s 5 + c 5 s 1 s 2 ) c 2 d 3 + d 6 (c 2 c 5 c 4 s 2 s 5 ) z 0 = z 1 = s 1 c 1 0 z 2 = z 3 = c 1s 2 s 1 s 2 c 2 z 4 = c 1c 2 s 4 s 1 c 4 s 1 c 2 s 4 + c 1 c 4 s 2 s 4 z 5 = c 1c 2 c 4 s 5 s 1 s 4 s 5 + c 1 s 2 c 5 s 1 c 2 c 4 s 5 + c 1 s 4 s 5 + s 1 s 2 c 5 s 2 c 4 s 5 + c 2 c 5

21 KINEMATIC SINGULARITIES v = J(q) q if J is rank-deficient = kinematic singularities (a) reduced mobility (b) infinite solutions to inverse kinematics problem (c) large joint velocities (in the neighbourhood of singularity) Classification Boundary singularities Internal singularities

22 Two link planar arm [ ] a1 s J = 1 a 2 s 12 a 2 s 12 a 1 c 1 + a 2 c 12 a 2 c 12 det(j) = a 1 a 2 s 2 ϑ 2 = 0 ϑ 2 = π [ (a 1 + a 2 )s 1 (a 1 + a 2 )c 1 ] T parallel to[ a 2 s 1 a 2 c 1 ] T (components of end-effector velocity non independent)

23 Singularity decoupling computation of arm singularities computation of wrist singularities

24 J = [ ] J11 J 12 J 21 J 22 J 12 = [ z 3 (p p 3 ) z 4 (p p 4 ) z 5 (p p 5 ) ] J 22 = [ z 3 z 4 z 5 ] p = p W = p W p i parallel to z i, i = 3, 4, 5 J 12 = [ ] det(j) = det(j 11 )det(j 22 ) det(j 11 ) = 0 det(j 22 ) = 0

25 Wrist singularities z 3 parallel to z 5 ϑ 5 = 0 ϑ 5 = π rotations of equal magnitude about opposite directions on ϑ 4 and ϑ 6 do not produce any rotation at the end-effector

26 Arm singularities Anthropomorphic arm det(j P ) = a 2 a 3 s 3 (a 2 c 2 + a 3 c 23 ) Elbow singularity s 3 = 0 a 2 c 2 + a 3 c 23 = 0 ϑ 3 = 0 ϑ 3 = π

27 Shoulder singularity p x = p y = 0

28 ANALYSIS OF REDUNDANCY Differential kinematics v = J(q) q if (J) = r dim ( R(J) ) = r dim ( N(J) ) = n r in general dim ( R(J) ) + dim ( N(J) ) = n

29 If N(J) where check: q = q + P q a R(P) N(J) J q = J q + JP q a = J q = v q a generates internal motions of the structure

30 INVERSE DIFFERENTIAL KINEMATICS Nonlinear kinematics equation Differential kinematics equation linear in the velocities Given v(t) + initial conditions = (q(t), q(t)) if n = r q = J 1 (q)v q(t) = t 0 q(ς)dς + q(0) (Euler) numerical integration q(t k+1 ) = q(t k ) + q(t k ) t

31 Redundant manipulators For a given configuration q, find the solutions q that satisfy v = J q and minimize g( q) = 1 2 qt W q method of Lagrange multipliers g( q, λ) = 1 2 qt W q + λ T (v J q) ( ) T g = 0 q ( ) T g = 0 λ optimal solution q = W 1 J T (JW 1 J T ) 1 v if W = I where q = J v J = J T (JJ T ) 1 is the right pseudo-inverse of J

32 Use of redundancy g ( q) = 1 2 ( qt q T a )( q q a) like above... g ( q, λ) = 1 2 ( qt q T a )( q q a ) + λ T (v J q) optimal solution q = J v + (I J J) q a

33 Characterization of internal motions ( ) T w(q) q a = k a q manipulability measure w(q) = det ( J(q)J T (q) ) distance from mechanical joint limits w(q) = 1 2n n ( qi q i i=1 q im q im ) 2 distance from an obstacle w(q) = min p,o p(q) o

34 Kinematic singularities The previous solutions hold only when J is full-rank If J is not full-rank (singularity) if v R(J) = solution q extracting all linearly independent equations ( physically executable path) if v / R(J) = the system of equations has no solution (path cannot be executed) Inversion in the neighbourhood of singularities det(j) small = q large damped least-squares inverse J = J T (JJ T + k 2 I) 1 where q minimizes g ( q) = v J q 2 + k 2 q 2

35 ANALYTICAL JACOBIAN p = p(q) φ = φ(q) ṗ = p q q = J P(q) q φ = φ q q = J φ(q) q ẋ = [ ] ṗ φ = [ ] JP (q) J φ (q) q = J A (q) q J A (q) = k(q) q

36 Angular velocity of Euler angles ZYZ about axes of reference frame as a result of ϕ: [ ω x ω y ω z ] T = ϕ [ ] T as a result of ϑ: [ ω x ω y ω z ] T = ϑ [ s ϕ c ϕ 0 ] T as a result of ψ: [ ω x ω y ω z ] T = ψ [ c ϕ s ϑ s ϕ s ϑ c ϑ ] T

37 Composition of elementary angular velocities ω = 0 s ϕ c ϕ s ϑ 0 c ϕ s ϕ s ϑ φ = T(φ) φ 1 0 c ϑ

38 Physical meaning of ω ω = [ π/2 0 0 ] T 0 t 1 ω = [ 0 π/2 0 ] T 0 t 1 ω = [ 0 π/2 0 ] T 1 < t 2 ω = [ π/2 0 0 ] T 1 < t ωdt = [ π/2 π/2 0] T

39 Relationship between analytical Jacobian and geometric Jacobian v = [ ] I O ẋ = T O T(φ) A (φ)ẋ J = T A (φ)j A Geometric Jacobian quantities of clear physical meaning Analytical Jacobian differential quantities in the operational space

40 INVERSE KINEMATICS ALGORITHMS Kinematic inversion q(t k+1 ) = q(t k ) + J 1 (q(t k ))v(t k ) t drift of solution Closed-Loop Inverse Kinematics (CLIK) algorithm operational space error e = x d x ė = ẋ d ẋ = ẋ d J A (q) q find q = q(e): e 0

41 Jacobian (pseudo-)inverse Linearization of error dynamics q = J 1 A (q)(ẋ d + Ke) ė + Ke = 0 For a redundant manipulator q = J A (ẋ d + Ke) + (I J A J A) q a

42 Jacobian transpose q = q(e) without linearizing error dynamics Lyapunov method V (e) = 1 2 et Ke where V (e) > 0 e 0 V (0) = 0 V (e) = e T Kẋ d e T Kẋ = e T Kẋ d e T KJ A (q) q the choice q = J T A(q)Ke leads to V (e) = e T Kẋ d e T KJ A (q)j T A(q)Ke if ẋ d = 0 = V < 0 with V > 0 (asymptotic stability) if N(J T A ) = V = 0 if Ke N(J T A ) q = 0 with e 0 (stuck?)

43 If ẋ d 0 e(t) bounded (worth increasing norm of K) e( ) 0

44 Example J T P = c 1 (a 2 s 2 + a 3 s 23 ) s 1 (a 2 s 2 + a 3 s 23 ) 0 a 3 c 1 s 23 a 3 s 1 s 23 a 3 c 23 null of J T P ν y ν x = 1 tanϑ 1 ν z = 0

45 Orientation error Position error e P = p d p(q) ė P = ṗ d ṗ Euler angles e O = φ d φ(q) ė O = φ d φ [ ] q = J 1 A (q) ṗd + K P e P φ d + K O e O handy to assign the time history φ d (t) anyhow requires computation of R = [ n s a ] Manipulator with spherical wrist compute q P = R W compute R T W R d = q O (Euler angles ZYZ)

46 Angle and axis R(ϑ, r) = R d R T orientation error e O = r sinϑ = 1 2 (n n d + s s d + a a d ) ė O = L T ω d Lω where L = 1 2 ( S(nd )S(n) + S(s d )S(s) + S(a d )S(a) ) ė = [ ėp ė O ] = = [ ṗ d J P (q) q L T ω d LJ O (q) q ] [ ] [ ] ṗ d I O L T J q ω d O L [ ] q = J 1 ṗ (q) d + K P e P L ( ) 1 L T ω d + K O e O

47 Unit quaternion Q = Q d Q 1 orientation error e O = ǫ = η(q)ǫ d η d ǫ(q) S(ǫ d )ǫ(q) [ q = J 1 ṗd + K (q) P e P ω d + K O e O ] quaternion propagation ω d ω + K O e O = 0 η = 1 2 ǫt ω ǫ = 1 2 (ηi S(ǫ)) ω study of stability V = (η d η) 2 + (ǫ d ǫ) T (ǫ d ǫ) V = e T O K Oe O

48 Second-order algorithms time differentiation of differential kinematics ẍ e = J A (q) q + J A (q, q) q joint accelerations solution q = J 1 A (ẍ (q) e J ) A (q, q) q ë + K D ė + K P e = 0

49

50 Comparison among inverse kinematics algorithms Three link planar arm p x p y φ x = k(q) = a 1c 1 + a 2 c 12 + a 3 c 123 a 1 s 1 + a 2 s 12 + a 3 s 123 ϑ 1 + ϑ 2 + ϑ 3 a 1 = a 2 = a 3 = 0.5 m J A = a 1s 1 a 2 s 12 a 3 s 123 a 2 s 12 a 3 s 123 a 3 s 123 a 1 c 1 + a 2 c 12 + a 3 c 123 a 2 c 12 + a 3 c 123 a 3 c

51 q i = [ π π/2 π/2] T rad p di = [ 0 0.5] T m φ = 0 rad desired trajectory p d (t) = [ ] 0.25(1 cosπt) 0.25(2 + sinπt) 0 t 4 φ d (t) = sin π 24 t 0 t 4 MATLAB simulation with Euler numerical integration q(t k+1 ) = q(t k ) + q(t k ) t and t = 1 ms

52 q = J 1 A (q)ẋ 2 x 10 3 pos error norm 0 x 10 5 orien error [m] [s] [rad] [s]

53 q = J 1 A (q)(ẋ d + Ke) K = diag{500, 500, 100} 5 joint pos 10 joint vel [rad] [rad/s] [s] [s] 1 x 10 5 pos error norm 0 x 10 8 orien error [m] [rad] [s] [s]

54 free φ (r = 2, n = 3) q = J P (ṗ d + K P e P ) K P = diag{500, 500} 5 x 10 6 pos error norm 0.5 orien 4 [m] 3 2 [rad] [s] [s] q = J T P (q)k Pe P K P = diag{500, 500} 0.01 pos error norm 0.5 orien [m] [rad] [s] [s]

55 q = J P (ṗ d + K P e P ) + (I J P J P) q a KP = diag{500, 500} manipulability measure w(ϑ 2, ϑ 3 ) = 1 2 (s2 2 + s 2 3) ( ) T w(q) q a = k a k a = 50 q 5 1 joint pos 5 1 joint pos [rad] [rad/s] [s] [s] 5 x 10 6 pos error norm 1 manip 4 [m] 3 2 [rad] [s] [s]

56 distance from mechanical joint limits 3 ( qi q i ) 2 w(q) = 1 6 i=1 q im q im 2π q 1 2π π/2 q 2 π/2 3π/2 q 3 π/2 ( ) T w(q) q a = k a k a = 250 q [rad] joint 1 pos [s] [rad] joint 2 pos [s] 5 joint 3 pos 2 x 10 4 pos error norm 1.5 [rad] 0 [m] [s] [s]

57 STATICS Relationship between end-effector forces and moments (forces) γ and joint forces and/or torques (torques) τ with manipulator at equilibrium configuration elementary work associated with torques dw τ = τ T dq elementary work associated with forces dw γ = f T dp + µ T ωdt = f T J P (q)dq + µ T J O (q)dq = γ T J(q)dq elementary displacements virtual displacements δw τ = τ T δq δw γ = γ T J(q)δq

58 Principle of virtual work the manipulator is at static equilibrium if and only if δw τ = δw γ δq τ = J T (q)γ

59 Kineto-statics duality N(J) R (J T ) R(J) N (J T ) forces γ N(J T ) not requiring any balancing torques

60 Physical interpretation of Jacobian transpose CLIK algorithm ideal dynamics τ = q elastic force Ke pulling end-effector towards desired pose in operational space effective only if Ke / N(J T )

61 Velocity and force transformation [ ṗ2 ω 2 ] = [ I S(r12 ) O I ][ ṗ1 ω 1 ] r 12 = R 1 r 1 12 ṗ 1 = R 1 ṗ 1 1 ṗ 2 = R 2 ṗ 2 2 = R 1 R 1 2ṗ2 2 ω 1 = R 1 ω 1 1 ω 2 = R 2 ω 2 2 = R 1R 1 2 ω2 2

62 [ ṗ2 2 ω 2 2 ] = [ R 2 1 R1 2S(r1 12 ) ] [ ṗ1 1 O R1 2 ω 1 1 ] v 2 2 = J2 1 v1 1 by virtue of kineto-statics duality γ1 1 = J2 1 T γ2 2 [ f 1 ] [ 1 R 1 ][ 2 O f 2 ] 2 = S(r12)R R2 1 µ 2 2 µ 1 1

63 MANIPULABILITY ELLIPSOIDS Velocity manipulability ellipsoid set of joint velocities of constant (unit) norm q T q = 1 redundant manipulator q = J (q)v v T( J(q)J T (q) ) 1 v = 1 Axes eigenvectors u i of JJ T = directions singular values σ i = λ i (JJ T ) = dimensions Volume proportional to w(q) = det ( J(q)J T (q) )

64 Two link planar arm Manipulability measure w = det(j) = a 1 a 2 s 2 max at ϑ 2 = ±π/2 max at a 1 = a 2 (for given reach a 1 + a 2 )

65 Velocity manipulability ellipses [m] [m] Singular values [m] max min [m]

66 Force manipulability ellipsoid set of joint torques of constant (unit) norm τ T τ = 1 γ T( J(q)J T (q) ) γ = 1 Kineto-statics duality a direction along which good velocity manipulability is obtained is a direction along which poor force manipulability is obtained, and vice versa

67 Two link planar arm Velocity manipulability ellipses [m] [m] Force manipulability ellipses [m] [m]

68 Manipulator mechanical transformer of velocities and forces from joint space to operational space transformation ratio along a direction for force ellipsoid α(q) = ( ) 1/2 u T J(q)J T (q)u transformation ratio along a direction for velocity ellipsoid β(q) = (u T( J(q)J T (q) ) 1 u ) 1/2 use of redundant degrees of freedom

69 Task compatibility of structure along a given direction writing velocity writing plane force throwing in bowling game force velocity throwing direction