Inverting: Representing rotations and translations between coordinate frames of reference. z B. x B x. y B. v = [ x y z ] v = R v B A. y B.

Transcription

1 Kinematics Kinematics: Given the joint angles, comute the han osition = Λ( q) Inverse kinematics: Given the han osition, comute the joint angles to attain that osition q = Λ 1 ( ) s usual, inverse roblems might be troublesome! Kinematics Inverting: Geometricall: close form solution eists in certain cases minimization: 1 * J = Λ ( q) q = argmin J Kinematic reunanc: more joints than constraints E.g. a rigi bo (han) in sace is escribe b 6 numbers (osition + orientation). robot (or human) arm might have 7 or more joints (egrees of freeom) q R 005 Reresenting kinematics Reresenting rotations an translations between coorinate frames of reference Rotation matri R ( R ) = I ( R ) = ( R ) = R 1 Orthogonal matri v = [?] v z z v = [ z ] v = R v = R = R [1,0,0] Eamle: rotation along the Z ais cosϑ = 0 cosϑ 0 cosϑ z = z R 005 Rigi bo transformations ( t) q( t) = (0) q(0) = constant Given that the object is: O R he motion of the bo is reresente b a famil of maings: g( t) : O R rigi islacement of the bo is: g : O R Where: ction on oints an vectors g ( v) = g( q) g( ) * v = q Note the ifference between oints an vectors (although both are reresente as -tules of numbers). vector has magnitue an irection an oesn t belong to a bo (free vector). 1

2 hen g : R R is a rigi bo transformation if: g( ) g( q) = q for all oints, q R * * * Length is reserve g ( v w) = g ( v) g ( w) for all vectors v, w R he cross rouct is reserve he inner rouct is also reserve, thus: v w = g* ( v) g* ( w) I.e. orthogonal vectors remain orthogonal Some more requirements Right hane coorinate sstems: z z = If a coorinate sstem is attache to a rigi bo unergoing rigi motion: v1, v, v attache in then b effect of g g ( v ), g ( v ), g ( v ) are attache in g( ) * 1 * * Rotation matri Rotation matri (lanar case) ab R = [ z ] ab ab ab ab Coorinates of the s rincial ais relative to is the inertial frame, is the bo frame ab a z a z ab a ab Eamle: rotation along the Z ais cosϑ 0 cosϑ R R,,, z R ab ab ab ab hen: = 0 an so forth... ab ab = = RR R R I et R = 1 for right-hane coorinate sstems a ab ab cosϑ = 0 ab a z a = z a ab ab he grou of rotations SO() he set of matrices with these roerties is enote: SO() which means Secial Orthogonal of size hat is: SO R RR I R () = { R : =,et = + 1} Orthogonal Secial SO() is a grou uner matri multilication 1. Closure R, R SO() R R SO(). Ientit I is the ientit element IR = R R. Inverse 1 1 RR = R R = I, R SO() 4. ssociativit ( R R ) R = R ( R R ) 1 1

3 More simle rotations Eamle: rotation along the Y ais cosϑ cosϑ Reresenting D rotations Sequences of elementar rotations Euler angles: z,, z or z,, z Roll, itch, aw angles: z,, Vector (ais of rotation) an angle cosϑ 0 cosϑ Eamle: rotation along the X ais Roto-translationtranslation Rotation combine with translation Homogeneous reresentation o make things uniform v = R v + o v = R v + o z o z v R o v = v = v im( v) = 4 Clearl Direct kinematics v = v C C C < > < e > 1 R o R R o = = Comosition of transforms Inverse of a rototranslation < 0 > < 1 > < > ( q ) ( q ) 0 n n 0 (,, z) = ( 1,,, 4) (0,0,0) e q q q q = Λ( q) orientation = Λ( ɶ q) n

4 Conventions For lacing the reference frames on each link Denavit-Hartenberg Man times DH arameters are given for a maniulator (an various useful equations are also given wrt DH convention) Inverse kinematics Direct aroach Geometric Minimization Neural network, learning Inverse kinematics Direct aroach r solving: = NL ( q, q, q, q ) 1 4 = NL ( q, q, q, q ) 1 4 z = NL ( q, q, q, q ) z 1 4 Geometric aroach For certain maniulator the solution eists in close form Decomosable structures (e.g. translation an rotations can be hanle searatel) Rotations follow certain rules Man inustrial maniulators were esigne with inverse kinematics in min for q, q, q, q 1 4 Minimization Fin the solution to: 1 * J = Λ ( q) q = argmin J Neural network/learning: ( q, ) Λ 1 roimate the inverse out of a famil of functions (NN aroach) starting from eamles q What about velocit? Jacobian matri 1 1 q1 q m q = Λ( q) = t t n n q1 qm q = J ( q) t t 4

5 Note on reresenting velocities If is: = (,, z, ϑ, ϕ, ψ ) Position + Euler angles v = ( v, v, v, ɺ ϑ, ɺ ϕ, ψɺ ) z Euler angles erivatives o not have an clear hsical meaning v = ( v, v, vz, ω) ngular velocit (rate of rotation along the ais nwa Just make sure the reresentation an the equations are consistent v = ( v, v, v, ɺ ϑ, ɺ ϕ, ψɺ ) J z r v = ( v, v, v, ω) J z v Jacobian Formula Given the DH reresentation of transformations Consiering onl rotational joints J J = [ J J J ] for n joints v z 1 o o i E, i i = o zi = o o o E, i E i < 0 > n < 1 > o i < > o E < > < e > Having written i i zi i = i = i 1 i 1 i When J is invertible Can comute the joint velocities to obtain a certain han velocit qɺ = J 1 ɺ If n>6, reunanc: + + qɺ = J ɺ + ( I J J ) k k is a constant vector roubles Even if n 6 there are man situations where J cannot be inverte (singularities) Movement singularities (chain of rotations) J not invertible because certain elements go to zero 5

6 Resolve rate controller * s - s qɺ 1 J 1 s Sensors q Joint controllers Static Relationshi between forces an torques = Jq q τ = F q τ = q J F τ = J F Imagining the integrals where aroriate nother iea J τ = F Use this equation to esign a force controller: Given F comute the torques to rive the joints Dnamics wo methos to erive the equation of motion (ifferential equations) Newton-Euler Lagrange formalism Newton-Euler Lagrange formulation Start from: F = ( m v) t τ = ( I ω) t F = ( m v) t τ = ( I ω) = ω ( I ω) + I ωɺ t kinematics Write own ever equation (6): fin the angular velocit an I with resect to a base frame Lagrange equations: L = K P µ L L Fµ = µ q i t q ɺi q i Eternal forces (no otential) = ( q q, t) µ µ 1 1 K = mv v + ω Iω 1 N 6

7 For a maniulator ake the joint angles as variable, write the osition of the links, write own K, P an the eternal forces Eternal forces (control) Inertia (generalize) τ = M ( q) qɺɺ + h( q, qɺ ) qɺ + g( q) Coriolis, centrifugal effects Gravit Comleit Newton-Euler: Lagrange: o( n) o n 4 ( ) Estimation Kinematics just measure the arams Dnamics estimate from ata Dnamics Direct namics: τ ( t) q( t) Simulation (integrate the equations Runge-Kutta, Euler, etc.) Inverse namics: q( t) τ ( t) Dnamics an control Case 1: arameters are such that feeback gain at each joint is >> gravit, Coriolis, centrifugal, isturbances, etc. Case : feeback in not enough for high-see, recision, etc. comensation is require Case 1 ro behavior: ɺɺ ɺ * q + q + k[ q q ] = 0 Can esign k or a PID controller to make this sstem behave as esire Case Let s imagine we know all the arameters with a certain recision: τ = M ( q) qɺɺ + h( q, qɺ ) qɺ + g( q) τcontrol = M ( q) u + h( q, qɺ ) qɺ + g( q) M ( q) qɺɺ + h( q, qɺ ) qɺ + g( q) = M ( q) u + h( q, qɺ ) qɺ + g( q) M ( q) qɺɺ = M ( q) u u = qɺɺ + k ( qɺ qɺ ) + k ( q q) * * * 7

8 Case (continue) qɺɺ = u u = qɺɺ + k ( qɺ qɺ ) + k ( q q) * * * qɺɺ = qɺɺ + k ( qɺ qɺ ) + k ( q q) * * * * e = q q ɺɺ e eɺ e 0 = + k + k e t roriate esign of the gains can get arbitrar eonential behavior of the error 8