Present Ac i on Cap i Tulo 2 Angeles

8/12/2019 Present Ac i on Cap i Tulo 2 Angeles

1/122

Fundamentals of Robotic MechanicalSystems

Chapter 2: Mathematical Background

Jorge Angeles

Department of Mechanical Engineering &Centre for Intelligent Machines

McGill University

2009

Jorge Angeles (McGill University ) Chapter 2: Mathematical Background 2009 1 / 122
http://find/


2/122

Outline

1 Preambule

2 Linear Transformations

3 Rigid-Body RotationsThe Cross-Product Matrix

The Rotation MatrixThe Linear Invariants of a 3 3 MatrixLinear Invariants of a RotationExamplesEuler-Rodrigues Parameters

4 Composition of Reections and Rotations

5 Coordinate Transformations and Homogeneous Coordinates

http://find/


3/122

Preamble

Links are regarded as rigid bodies

General motion : rotation and translation

Invariant concepts : items that do not change upon a change of coordinate

frameExamples : distances between points and angles between lines

Vectors vs. components

Tensors vs. matricesWe will use matrix whenever we mean tensor .

http://find/


4/122

Linear Transformations

Vector space : Set of objects called vectorsthat follow certain algebraic rules

The physical 3-dimensional space is a particular case of a vector space

vectors : in boldface lower case

tensors and matrices : in boldface upper case

http://find/


5/122

Algebraic Rules for Vectors

Let v , v1, v2, v3 and w be elements of a vector space V over the real eldwhile and are real numbers :(i) v 1 + v 2 = v2 + v 1

(ii ) v + 0 = v where 0 : the zero vector

(iii ) v 1 + ( v 2 + v 3) = ( v 1 + v 2) + v 3(iv) v + w = 0 w = v(v) ( v ) = ( )v

(vi) = 1 v = v(vii ) ( + )v = v + v

(v 1 + v 2 ) = v 1 + v 2

http://find/http://goback/


6/122

Linear Transformations (Contd)

L : linear transformation operator over the real eld

v = Lu

Properties of linear transformations :(i) homogeneity : L(u ) = v

(ii ) additivity : L(u 1 + u 2) = v1 + v 2

Range of L : v = Lu for every u in U Kernel of L : u N of U mapped by L into 0 V Examples of transformations in 3-dimensional Euclidean space E 3 :projections, reections, rotations, afne transformations



7/122

Linear Transformations (Contd)Orthogonal projection P of E 3 onto itself : linear transformation of E 3 onto aplane passing through the origin with unit normal n . Properties :

P 2 = P : idempotent ; Pn = 0 : singular (2.1a)The projection p of p onto a plane of unit vector n :

F IG .: Projection.

p = p n (n T p ) = ( 1 nn T )p = Pp (2.1b)P = 1

nn T (2.4)

http://find/


8/122


Reection R of E 3 onto a plane passing through the origin with a unitnormal n :

F IG .: reection.

p = p

2nn T p

(1

2nn T )p = Rp

R = 1 2nn T (2.5)

Properties of reections : R 2 = 1, R 1 = R



9/122


Orthogonal decompositionv = v + v

axial component (v-par) :v

ee T v (2.6a)

normal component (v-perp)v v v (1 ee T )v (2.6b)

Vector space

Basis of a vector space V : set of linearly independent vectors {v i }n1 where nis the dimension of V v = 1v 1 + 2v 2 + + n v n (2.7)

http://find/


10/122


A linear transformation of U into V with bases BU = {u j }m1 and

BV =

{v i

}n

1 :

Lu j = l1 j v 1 + l2 j v 2 + + lnj v n j = 1 , . . . , m

[L ]BVBU

l11 l12

l1m

l21 l22 l2m... ... . . . ...ln 1 ln 2 lnm

(2.10)

DenitionThe j th column of L with respect to BU and BV is composed of the n realcoefcients lij of the representation of the image of the j th vector of B U interms of BV .

http://find/


11/122


If Le = e ,

then e : eigenvector of L ,

: eigenvalue of L

Characteristic polynomial of L :

det( 1 L ) = 0 (2.11)



12/122

Example

ExampleWhat is the representation of the reection R of E 3 into itself, with respect tothe x-y plane, in terms of unit vectors parallel to the X, Y, Z axes that formthe coordinate frame F ?

Ri = i, Rj = j, Rk =

k

[Ri ]F =100

, [Rj ]F =010

, [Rk ]F =00

1

[R ]F =

1 0 0

0 1 00 0 1

http://find/


13/122

Rigid-Body Rotations

Linear isomorphism : one-to-one linear transformation mapping space V ontoitself Isometry : distance between any two points is preserved :

d

(u

v )T (u

v ) (2.12)

where u and v are position vectors of two points.

Volume of tetrahedron formed by triad {u , v , w}:V 16 |u v w | = 16 det u v w (2.13)

http://find/


14/122

Rigid-Body Rotations (Contd)

Magnitudes of vectors {u , v , w}:u u T u , v v T v , w w T w (2.14)

Under isotropy mapping {u , v , w}into {u , v , w }:u = u , v = v , w = w (2.15a)

det[u v w ] = det[u v w ] (2.15b)If + , then rotation ; otherwise, reection

http://goforward/http://find/http://goback/


15/122

Rigid-Body Rotations ( Contd )

A position vector p of a point in E 3 undergoes a rotation Q :p = Qp , p T p = p T p

Q T Q = 1 and Q is an orthogonal matrix .

Moreover, for T

[u v w ] and T

[u v w ],

T = QT (2.20)

det(T ) = det( T ) (2.21a)

det( Q ) = +1 (2.21b)

Q is a proper orthogonal matrix.



16/122


TheoremThe eigenvalues of a proper orthogonal matrix Q lie on the unit circlecentered at the origin of the complex plane.

Proof :Qe = e , eQ = e

: eigenvalue of Q , e : eigenvector of Q : complex conjugate of , e : complex conjugate of e

For real Q : Q = Q T , e = eT

e T QQe = e T e eT Q T Qe = e T e (2.25)Q is orthogonal eT e = | |2 e T e | |2= 1 (2.27)

http://find/


17/122


Corollary

A proper orthogonal 3 3 matrix has at least one eigenvalue that is +1 .

Qe = e (2.28)

Theorem(Euler, 1776)

A rigid-body motion about a point O leaves xed a set of points lying on aline Lthat passes through O and is parallel to eigenvector e of Q associated with the eigenvalue +1 .

http://find/


18/122


Theorem(Cayley-Hamilton) If P () is the characteristic polynomial of n n matrixA , i.e.,

P () = det( 1 A )=

n

+ an 1n

1

+ + a1 + a0 (2.29)then,A n + an 1A

n 1 + + a1A + a01 = O (2.30)O : n n zero matrixAny power p n of the n n matrix A can be expressed as linearcombination of the rst n powers of A :

1 , A , , A n 1 where 1 = A 0

http://find/


19/122

The Cross-Product Matrix

If u = u (t ) U and v = v (u (t )) V are m- and n-dimensional vectors,respectively, while t is a real variable, and f = f (u , v ) is a real-valuedfunction, then

u (t) =

u1(t)u2(t)

...um (t)

, v (t) =

v1(t)v2(t)

...vn (t)

http://find/


20/122

Cross-Product Matrix ( Contd )

f u

f/u 1

f/u 2...f/u m

, f v

f/v 1

f/v 2...f/v n

(2.31)

v u

v1 /u 1 v1 /u 2 v1 /u mv2 /u 1 v2 /u 2 v2 /u m... ... . . . ...vn /u 1 vn /u 2 vn /u m

(2.32)

df du

= f u

+ v u

T f v

(2.33)


d ( )
http://find/


21/122


If f = f (u , v , t ) and v = v (u , t ), then

df dt

= f

t +

f u

T dudt

+f v

T vt

+ f v

T v u

dudt

(2.34)

and

dvdt =

vt +

v u

dudt (2.35)


E l
http://find/


22/122

Example

Example

Let the components of v and x in certain reference frame F be given as[v ]F =

v1v2v3

, [x ]F =x1x2x3

(2.36a)

what are the components of [v x ] and (v

x

) x in the same frame F ?Solution :[v x ]F =

v2x3 v3x2v3x1 v1x3v1x2

v2x1 (2.36b)

(v x )

x F =

0 v3 v2v3 0 v1v2 v1 0

(2.36c)


C P d t M t i ( )


23/122


v

x = Vx (2.37)

TheoremThe cross-product matrix A of any 3-dimensional vector a is skew-symmetric,i.e.,

A T = A a (a b ) = A 2b (2.38)with

A 2 = a 21 + aa T (2.39)

and being the Euclidean norm of the vector.The cross-product matrix (CPM) A of any 3-dimensional vector a is uniquelydened.


Th R t ti M t i
http://find/


24/122

The Rotation Matrix

F IG .: Rotation of a rigid body about a line.

p = OQ + QP (2.40)OQ : the axial component of p along eQP : normal component of p with respect to e


Rotation Matrix ( C d )
http://find/


25/122

Rotation Matrix ( Contd )

OQ = ee T p (2.41)QP = (cos ) QP +(sin ) QP (2.42)

QP = ( 1 ee T )p ,QP = e p Ep

QP = cos (1 ee T )p + sin Ep (2.45)

p = [ee T + cos (1 ee T ) + sin E ]p (2.47)p is a linear transformation of p with a rotation matrix

Q = ee T + cos (1 ee T ) + sin E (2.48)e , : The natural invariants of Q


Rotation Matrix ( C td )


26/122


Special cases :

= Q = 1 + 2 ee T (2.49) = 0

Q = 1

leading to a symmetric Q .

Under no circumstance does the rotation matrix become skew-symmetric, fora 3

3 skew-symmetric matrix is by necessity singular.


http://find/


27/122


The series expansion of Q for a xed value of e :

Q () = Q (0) + Q (0) + 12!

Q (0)2 + + 1k!Q(k ) (0)k +

where (k) is the kth derivative of Q with respect to .

Since Q(k )

(0) = Ek

, E(2k+1)

= ( 1)k

E ,E 2k = ( 1)k (1 ee T ),

Q () = 1 + E + 1

2!E 22 +

+

1

k!E k k +

Q () = eE (2.53)


http://find/


28/122

Rotation Matrix ( Cont d )

Q () = 1+[

12!

2 + 14!

4 + 1(2k)!

(1)k 2k +

cos 1

](1 ee T )

+[ 13!3 + + 1(2k + 1)! (1)k 2k+1 + sin

]E

Q = 1 + sin E + (1 cos )E2

(2.54)




29/122

Rotation Matrix ( Cont d )Canonical Forms of the Rotation Matrix

If X axis is parallel to the axis of rotation, i.e., to [e ]

X = 1 0 0 T , then

[E ]X =0 0 00 0 10 1 0

, [E 2]X =0 0 00 1 00 0 1

[Q ]X =1 0 00 cos sin 0 sin cos

(2.55a)

[Q

]Y =

cos 0 sin 0 1 0

sin 0 cos (2.55b)

[Q ]Z =cos sin 0sin cos 0

0 0 1 (2.55c)


The Linear Invariants of a 3 3 Matrix
http://find/


30/122

The Linear Invariants of a 3 3 MatrixFor any 3 3 matrix A :

Cartesian decomposition

A S 12

(A + A T ), A SS 12

(A A T ) (2.56)A S : symmetric part, A SS : skew-symmetric part.

The axial vector is a vector a with the property

a v A SS v (2.57)for any vector v .

The trace of A is the sum of the eigenvalues of A S .


Linear Invariants of a 3 3 Matrix ( Contd )


31/122

Linear Invariants of a Matrix ( Cont d )

Let entries of matrix A be aij , for i, j = 1 , 2, 3. Then,

vect( A )

a

1

2

a32 a23a13

a31a21 a12

tr( A )

a11 + a22 + a33 (2.58)


http://find/


32/122


TheoremThe vector of a 3 3 matrix vanishes if and only if it is symmetric, whereasthe trace of an n n matrix vanishes if the matrix is skew symmetric.For 3-dimensional vectors a and b :

vect( ab T ) = 12

a b (2.59)

tr( abT

) = aT

b (2.60)




33/122


Proof of relation ( 2.59 ) :Let w denote vect (ab T )

w v = Wv (2.61)

W : skew-symmetric component of ab T

W 12

(ab T ba T ) (2.62)

Wv = w

v =

1

2[(b T v )a

(a T v )b ] (2.63)


http://find/


34/122

( )

Compare relation ( 2.63 ) with the double-cross product :

(b a ) v = ( b T v )a (a T v )b (2.64)

w =

12b a (2.65)

The trace of an n n matrix can vanish without the matrix necessarilybeing skew-symmetric, while the trace of a skew-symmetric matrix

necessarily vanishes.


Linear Invariants of a Rotation


35/122

Matrices 1, ee T and cos (1 ee T ) are symmetric ;matrix sin E is skew-symmetric . Hence,

vect( Q ) = vect(sin E ) = sin e (2.66)

tr( Q ) = tr[ ee T + cos (1 ee T )] eT e + cos (3 e T e )= 1 + 2 cos (2.67)

cos = tr( Q ) 12 (2.68)


Linear Invariants of a Rotation (Contd)


36/122

( )

vect (Q ) q =q 1q 2q 3

in a given frame.

Introducing q 0 cos as a linear invariant of the rotation matrix , the rotationmatrix can be fully dened by four scalar parameters : [q 1, q 2, q 3, q 0]T (2.69)

These components of are not independent since

q 2 + q 20 sin2 + cos 2 = 1 (2.70)


Linear Invariants of a Rotation (Contd)


37/122

2 q 21 + q 22 + q 23 + q 20 = 1 (2.71)

sin = q , e = q / sin (2.73)

Q = qq T

q 2 + q 0 1 qq T

q 2 + Q (2.74a)

Q (q x )

x

Q = q 01 + Q + qq T

1 + q 0 (2.74b)


Linear Invariants of a Rotation
http://find/


38/122

Linear invariants are not suitable to represent a rotation when is either or close to it.

Changing the sign of e does not change the rotation matrix, provided that the

sign of is also changed.Choose 0

sin =

q , e =

q

q


Examples
http://find/


39/122

ExampleIf [e ]F = [ 3/ 3, 3/ 3, 3/ 3 ]T in a given coordinate frame F and = 120, what is Q in F ?Solution : From the data,

cos = 12

, sin = 32

[eeT

]F =

1

3

1

11 1 1 1 = 1

3

1 1 11 1 11 1 1


Examples ( Contd )


40/122

[1 ee T ]F = 13

2 1 11 2 11 1 2

, [E ]F 33

0 1 11 0 11 1 0

[Q ]F = 13

1

1 1

1 1 11 1 1 16

2 1

1

1 2 11 1 2

+ 36

0

1

1

1 0 11 1 0

[Q ]F =0 1 00 0 11 0 0


Examples ( Contd )
http://find/


41/122

ExampleIs the linear transformation below a rotation ?

[Q ]F =0 1 00 0 11 0 0

If so, nd its natural invariants.

Solution : Test for orthogonality :

[Q ]F [QT

]F =0 1 0

0 0 11 0 0

0 0 1

1 0 00 1 0

=

1 0 0

0 1 00 0 1


Examples ( Contd )
http://find/


42/122

det( Q ) = +1 a rotationFind e and : vect( Q ) = (sin )e

[q ]F sin [e ]F = 12

1 1 1 T

sin q = 32 = 60 or 120

[e ]F = [q ]F

q =

33

1 1 1 T

1 + 2 cos tr( Q ) = 0 , cos = 12 = 120


Examples ( Contd )
http://find/


43/122

Example

A coordinate frame X 1, Y 1, Z 1 is rotated into a conguration X 2, Y 2, Z 2 insuch a way that

X 2 = Y 1, Y 2 = Z 1, Z 2 = X 1Find the matrix representation of the rotation in X 1, Y 1, Z 1 coordinates.Compute the direction of the axis and the angle of rotation.

Solution : Let i1, j1, k 1 be unit vectors parallel to X 1, Y 1, Z 1, respectively,i2, j2, k2 being dened correspondingly.

i2 = j1, j2 = k1, k2 = i1


Examples ( Contd )
http://find/


44/122

[Q ]1 =

0 0 11 0 00 1 0

[q ]1 [ vect(Q ) ]1 = sin [e ]1 = 121

1

1

cos = 12 [tr( Q ) 1 ] = 12Assume sin 0 sin = q =

32

[e ]1 = [q ]1sin = 331

11 = 120


Examples ( Contd )
http://find/


45/122

Example

Show that the matrix P in eq. ( 2.4) satises ( 2.1a ).

Solution : Is P idempotent ?

P 2 = ( 1

nn T )(1

nn T )

= 1 2nn T + nn T nn T = 1 nn T = PMoreover,

Pn = ( 1

nn T )n = n

nn T n = n

n = 0

n is eigenvector of P with eigenvalue 0 and, hence, spans the nullspaceof P


Examples ( Contd )
http://find/


46/122

ExampleThe representations of three linear transformations in a given coordinateframe F :

[A ]F = 1

3

2 1 2

2 2 1

1 2 2, [B ]F =

1

3

2 1 11 2

1

1 1 2,

[C ]F = 13

1 2 22 1 22 2 1

Identify the orthogonal projection, the reection and the rotation and nd itsinvariants.


Examples ( Contd )
http://find/


47/122

Solution : B and C are symmetric. Rotation matrix is symmetric if and only if sin = 0 i.e., = 0 or , with trace 3 or 1. But

tr( B ) = 2 , tr( C ) = 1

B and C are not rotations

[AA T ]F = 19

9 0 00 9 00 0 9

, det( A ) = +1

A is a rotation


Examples ( Contd )
http://find/


48/122

Natural invariants of A :

sin [e ]F = [vect( A )]F = 12

111

cos = 12

[tr( A )

1] = 1

2(2

1) = 1

2

For sin 0 sin = vect( A ) = 32 = 60

[e ]F

= [vect(A )]F

vect( A ) =

33

111


Examples ( Contd )
http://find/


49/122

Test B and C for orthogonality :

[BB T ]F = 13

2 1 11 2 11 1 2

= [ B 2 ]F = [ B ]F

B is not orthogonal but idempotent

[CC T ]F = 19

9 0 00 9 00 0 9

C is orthogonal


Examples ( Contd )
http://find/


50/122

det (C ) = 1 C is a reectiondet (B ) = 0 B is singular : a projection

The unit vector [n ]F = [ n1, n2, n3 ]T

spanning the nullspace of B can bedetermined from :Bn = 0 nn T = 1 B

Produces two solutions, one the negative of the other.


Examples ( Contd )
http://find/


51/122

nn T =n21 n1n2 n1n3

n1n2 n22 n2n3n1n3 n2n3 n23

= 13

1 1 11 1 11 1 1

n =

33

1

11

From C = 1

2nn T follows the same solution for n


Examples ( Contd )
http://find/


52/122

ExampleThe vector and the trace of a rotation matrix Q in a reference frame F are

[vect( Q )]F = 12

111

, tr( Q ) = 2

Find the matrix representation of Q in the given coordinate frame and in aframe with its Z -axis parallel to vect( Q ).


Examples ( Contd )
http://find/


53/122

Solution :

Use eq.( 2.74a) to determine rotation matrix Q

[qqT

]F = 14

1 1 11 1 11 1 1

, q2

= 34

[Q ]F = 12

0 1 1

1 0 11 1 0

[Q ]F = 13

2 1 2

2 2 11 2 2


Examples ( Contd )
http://find/


54/122

Denote by Z a coordinate frame with Z axis parallel to q

[q ]Z = 32

001

, [qq T ]Z = 34

0 0 00 0 00 0 1

[Q ]Z = 32

0 1 01 0 00 0 0

[Q ]Z =1/ 2 3/ 2 0 3/ 2 1/ 2 00 0 1


Examples ( Contd )
http://find/


55/122

ExampleA procedure for trajectory planning produced a matrix

[Q ] =0.433 0.500 zx 0.866

0.433

0.866 y 0.500

representing a rotation for a certain pick-and-place operation where x, y, andz are entries that are unrecognizable due to failures in the printing hardware.Find the missing entries.


Examples ( Contd )
http://find/


56/122

Solution : Product Q T Q must be 1 :

Q T Q =0.437 + z2 0.433(x z 1) 0.5(y + z) + 0 .375 0.937 + x

2 0.866(x + y) 0.216 1 + y2

Upon equating the diagonal elements to unity :

x = 0.250, y = 0 , z = 0.750while the vanishing of the off-diagonal entries leads to :

x = 0 .250, y = 0 , z =

0.750

det( Q ) = +1


Euler-Rodrigues Parameters
http://find/


57/122

r sin2

e, r0 = cos2

(2.75)

Q = ( r 02 r r )1 + 2 rr T + 2 r 0R (2.76)where, for arbitrary x,

R (r x )

x

= : r0 = 1 + q 02 , r = q2r 0 (2.77) = : r0 = 0 , r = e (2.78)


The Matrix Square Root
http://find/


58/122

The square root for a 3 3 matrix is a linear combination of 1 (the identitymatrix), the matrix itself and its square, the coefcients being found using theeigenvalues of the matrix (from Cayley-Hamil- tons Theorem).

Square root of a rotation matrix : a rotation through angle / 2 about axisparallel to e.

http://find/


59/122

The Matrix Square Root ( Contd )


60/122

Not all 23 = 8 square roots of a 3 3 proper orthogonal matrix are properorthogonal but there is one and only one that represents a rotation of / 2denoted Q :

r = vect( Q ), r0 = tr( Q ) 12 (2.79)


Euler-Rodrigues Parameters ( Contd )
http://find/


61/122

Linear invariants can be derived from the rotation matrix by simple additions

and subtractions, but fail to produce information on the axis of rotation when = .

Euler-Rodrigues parameters require square roots and entail sign ambiguities,but produce information on the axis of rotation for any value of the rotationangle .

Note : Not to be confused with Euler angles , which are not invariant andadmit multiple denitions.

No single set of Euler angles exists for a given rotation ; set depends on howthe rotation is decomposed into three simpler rotations.


Example
http://find/


62/122

Example

Find the Euler-Rodrigues parameters of the proper orthogonal matrix

Q = 13

1 2 22 1 22 2 1

Solution :Q is symmetric = . Impossible to determine the direction of theaxis of rotation from the linear invariants. However, note that

ee T = 12

(1 + Q )


Example ( Contd )
http://find/


63/122

e21 e1e2 e1e3e1e2 e22 e2e3e1e3 e2e3 e23

= 13

1 1 11 1 11 1 1

e =

33 1 1 1

T

r = e sin2

= 33

111

, r0 = cos2

= 0


Composition of Reections and Rotations
http://find/


64/122

Let R be a pure reection & Q a rotation

T = QR and T = RQ

are neither symmetric nor self-inverse, yet both are reections !

An improper orthogonal transformation (not symmetric) can always be

decomposed into the product of a rotation and a pure reection . Thisdecomposition is not unique.

For arbitrary n : R 1 2nn T

Q = TR 1

TR = T

2(Tn )n T


Example
http://find/


65/122

ExamplePlace both palms in the diving position. Hold a sheet of paper between them. Thesheet denes a hand plane in each hand , its unit normal, hand normal , pointingoutside of the hand, n R and n L for the right and left hand, respectively.o R and o L : unit vectors pointing in the direction of the nger axes of each of thehands

n L = n R , o L = oR

Now, without moving your right hand, let the left hand attain a position whereby theleft-hand normal lies at right angles with the right-hand normal, the palm pointingdownwards and the nger axes of the two hands remaining parallel.Find the representation of the transformation carrying the right hand to the nalconguration of the left hand, in terms of the unit vectors n R and o R .


Example ( Contd )
http://find/


66/122

Solution : Desired transformation :

T = QR

Reection mapping the right hand into the left hand :

R = 1 2n R n T RLeft hand rotates from diver position about an axis ||o R through an angle of 90 cw. From eq.( 2.48 ) and above information,

Q = oR o T R + O R

where O R is the cross-product matrix of o R

T = oR o T R + 2O R 2(o R n R )n T R


Coordinate Transformations andHomogeneous Coordinates
http://find/


67/122

Coordinate Transformations Between Frameswith a Common Origin

A =

{X, Y, Z

} ,

B =

{X ,

Y ,

Z},

Q : A B (2.80)[p ]A = x y z

T(2.81)

Find [p ]B in terms of [p ]A and Q


Coordinate Transformations BetweenFrames with a Common Origin
http://find/


68/122

F IG .: Coordinate transformation : (a) coordinates of point P in the A-frame ; and (b)relative orientation of frame B with respect to A.


Coordinate Transformations BetweenFrames with a Common Origin ( Contd )
http://find/


69/122

Point P is attached to frame A, while frame Aundergoes a rotation Q aboutits origin and is carried to frame B :

= Qp (2.82)

where is the position vector of point : rotated position of P

[ ]B = x y zT

(2.83)

[ ]A = T (2.84)


http://find/


70/122

TheoremThe representations of the position vector of any point in two frames Aand B , denoted by [ ]A and [ ]B respectively, are related by

[ ]A

= [ Q ]A

[ ]B

(2.85)

Proof :[ ]A = [ Q ]A[p ]A (2.86)

[ ]B = [ p ]A (2.87)

[ ]A = [ Q ]A[ ]B (2.88)


http://find/


71/122

TheoremThe representations of Q carrying Ainto B in these two frames are identical,i.e.,

[Q ]A = [ Q ]B (2.89)

Proof :

[Qp ]A = [ Q ]A[Qp ]B, [Q ]A[p ]A = [ Q ]A[Qp ]B

[p ]A

= [ Q ]B

[p ]B

(2.90)

[p ]A = [ Q ]A[p ]B (2.91)




72/122

Theorem

The inverse relation of Theorem 1 is given by

[ ]B = [ QT ]B[ ]A (2.92)


Example
http://find/


73/122

Example

F IG .: Coordinate frames Aand B with a common origin.

Find the representations of Q rotating Ainto B in these two frames and showthat they are identical. Moreover, if [p ]A = [ 1, 1, 1 ]T , nd [p ]B.


Example ( Contd )
http://find/


74/122

Solution : Let i, j , and k be unit vectors in the directions of the X -, Y -, andZ -axes, respectively,

unit vectors , , and being dened likewise, as parallel to the X -, Y -, andZ -axes of Fig. 2.2

Q i = k , Q j = i, Q k = j

[Q ]A =0 1 00 0 1

1 0 0


Example ( Contd )
http://find/


75/122

Q = 0 1 00 0 11 0 0

00

1 = 010

= j =

Likewise,Q =

, Q =

From Denition 1

[Q ]B =0 1 00 0 11 0 0

= [ Q ]A


Example ( Contd )
http://find/


76/122

If we represent this matrix in a frame whose X -axis is directed along the axisof rotation of Q ,

[Q ]X =1 0 00 cos sin 0 sin cos

with = 120

[Q ]X =1 0 00 1/ 2 3/ 20 3/ 2 1/ 2

[p ]B

=0 0 1

1 0 0

0 1 0

111

=1

1

1


Coordinate Transformation with Origin Shift
http://find/


77/122

TheoremThe representations of the position vector p of a point P of the Euclidean3-dimensional space in two frames Aand B are related by

[p ]A = [ b ]A + [ Q ]A[ ]B (2.93a)

[ ]B = [ QT ]B([b ]A + [ p ]A) (2.93b)

with b dened as the vector directed from the origin of

Ato that of

B , and

the vector directed from the origin of B to P , as depicted in Fig. 2.3.


Coordinate Transformation with Origin Shift ( Contd )
http://find/


78/122

F IG .: Coordinate frames with different origins.


Coordinate Transformation with Origin Shift ( Contd )
http://find/


79/122

Proof :p = b + (2.94)

[p ]A = [ b ]A + [ ]A is assumed to be readily available in B

[p ]A = [ b ]A + [ Q ]A[ ]BTo prove eq.( 2.93b ), we simply solve eq.( 2.94 ) for and apply eq.( 2.92) to

the equation thus resulting.


Example
http://find/


80/122

Example

If [b ]A = [ 1, 1, 1 ]T and A& B have the relative orientations as inExample 4, nd the position vector, in B, of a point P of position vector [p ]Agiven as in the same example.

Solution : Find [ ]B : [b ]A + [ p ]A = 2 2 2T

By virtue of Theorem 2, Q ]B = [ QT ]A :

[Q ]

B =

0 0 1

1 0 0

0 1 0 [ ]

B =

0 0 1

1 0 0

0 1 0

222

= 2

2

2


Homogeneous Coordinates

G l di t t f ti i l i hift f th i i i t li
http://find/


81/122

General coordinate transformation involving a shift of the origin is not linear,e.g., the rst terms of the RHS of eq.( 2.93a ) is nonhomogeneous . It can berepresented in homogeneous form by introducing homogeneous coordinates :

{p }M [p ]T M 1T

(2.95)

Then, the afne transformation of eq.(2.93a) becomes

{p

}A =

{T

}A{

}Bwith :

{T }A [Q ]A [b ]A

0 T 1 , (2.97)

{T 1

}B =[Q T ]

B [

b ]

B0 T 1 (2.98)


Homogeneous Coordinates ( Contd )

Fk for k = i 1 i i + 1 : coordinate frames with origins at Ok
http://find/


82/122

F k , for k = i 1, i, i + 1 : coordinate frames with origins at OkQ i1 : the rotation of F i1 into F iQ i : the rotation of F i into F i+1If three origins coincide :

[p ]i = [ QT i1 ]i1[p ]i1[p ]i+1 = [ Q T i ]i [p ]i = [ Q T i ]i [Q T i1 ]i1[p ]i1

with inverse relation

[p ]i1 = [ Q i1 ]i1[Q i ]i [p ]i+1



If origins do not coincide a i 1 = Oi 1Oi a i = Oi Oi +1


83/122

If origins do not coincide, a i1 Oi1Oi , a i Oi Oi +1

{T i1}i1 = [Q i1 ]i1 [a i1 ]i10 T 1 ,

{T i}i =[Q i ]i [a i ]i

0 T 1

Inverse transformation :

{T i1}1i =[Q T i1 ]i [Q

T i1 ]i [a i1 ]i10 T 1

{T i}1

i+1 =[Q T i ]i+1 [Q T i ]i+1 [

a i ]i

0 T 1


http://find/


84/122

Therefore, the coordinate transformations are

{p i1}i1 = {T i1}i1{p i}i (2.105){p i1}i1 = {T i1}i1{T i}i{p i+1 }i+1 (2.106)

with the corresponding inverse transformations

{p i }i = {T i1}1i1{p i1 }i1 (2.107){p i +1 }i +1 = {T i }1i {p i }i = {T i }1i {T i1}1i1{p i1 }i1 (2.108)


http://find/


85/122

If P lies at innity :

{p }M p [e ]M1/ p

limp {

p

}M = lim

p p lim

p

[e ]M1/ p

= limp

p [e ]M0


http://find/


86/122

We dene the homogeneous coordinates of a point P lying at innity as

{p }M [e ]M

0 (2.109)

Let Q = e 1 e2 e3 and triad

{e k

}31 be orthonormal

{T }A of eq.( 2.97 ) becomes

{T }A =e 1 e2 e3 b0 0 0 1 (2.110)


Example


87/122

ExampleFind the equation of an ellipsoid in Awith semiaxes of a = 1 , b = 2 , c = 3and with its centre OB of position [b ]A = [ 1, 2, 3 ]

T , its axes X , Y , Z dening a coordinate frame B. The direction cosines of X are(0.933, 0.067, 0.354), whereas Y b and Y u while u ||X -axis.u , v , w are unit vectors parallel to the X -, Y -, and Z -axes, respectively :

[u ]A =0.9330.067

0.354

, v = u b

u

b

, w = u v


Example ( Contd )


88/122

[v ]A =0.243

0.8430.481, [w ]A =

0.2660.5350.803

[Q ]A = [ u , v , w ]A =0.933 0.243 0.2660.067 0.843 0.5350.354 0.481 0.803


Example ( Contd )
http://find/


89/122

If [p ]

A = [ p1, p2, p3 ]T and [ ]

B = [ 1, 2, 3 ]T ,

B: 21

12 +

2222

+ 2332

= 1

[QT

]B = [ QT

]A =0.933 0.067 0.3540.243 0.843 0.4810.266 0.535 0.803

[ ]B =0.933 0.067 0.3540.243 0.843 0.4810.266 0.535 0.803

123

+ p1 p2

p3


Example ( Contd )

Therefore
http://find/


90/122

Therefore,

1 = 0 .933 p1 + 0 .067 p2 0.354 p3 0.0052 = 0 .243 p1 0.843 p2 + 0 .481 p33 = 0.266 p1 0.535 p2 0.803 p3 + 3 .745

Upon substitution of the foregoing relations into the ellipsoid equation in B,A: 32.1521 p1 2 + 7 .70235 p22 + 9 .17286 p3 2 8.30524 p1

16.0527 p2 23.9304 p3 + 9 .32655 p1 p2 + 9 .02784 p2 p319.9676 p1 p3 + 20 .101 = 0


Similarity Transformations
http://find/


91/122

v = 1a 1 + 2a 2 + + n a nv = 1b 1 + 2b 2 + + n b n

[v ]A =

12...

n

, [v ]B =

1 2...

n

(2.113)

b j

= a1 j

a1 + a

2 ja

2 +

+ a

nja

n, j = 1 , . . . , n


Similarity Transformations ( Contd )

( )
http://find/


92/122

v = 1(a11 a 1 + a21a 2 + + an 1a n )+ 2(a12a 1 + a22a 2 + + an 2a n )...+ n (a1n a 1 + a2n a 2 + + ann a n )

v = ( a11 1 + a12 2 + + a1n n )a 1+ ( a21 1 + a22 2 + + a2n n )a 2

...

+ ( an 1 1 + an 2 2 + + ann n )a n


http://find/


93/122

[v ]A = [ A ]A[v ]B (2.117)where

[A ]A

a11 a12 a1na21 a22 a2n...

.

.. .

. . .

..an 1 an 2 ann

(2.118)

Inverse relationship :[v ]B = [ A

1 ]A[v ]A (2.119)



l11 l12 l1n
http://find/


94/122

[L ]A =

l11 l12 l1nl21 l22 l2n... ... . . . ...ln 1 ln 2 lnn

(2.120)

We want to nd relation between [L ]

A and [L ]

B. To this end, let

Lv = w (2.121)

whose representations in Aand B are

[L ]A[v ]A = [ w ]A (2.122)[L ]B[v ]B = [ w ]B (2.123)

http://find/


95/122


Theorem


96/122

Theorem

The characteristic polynomial of a given n n matrix remains unchanged under a similarity transformation. Moreover, the eigenvalues of two matrixrepresentations of the same n n linear transformation are identical, and if [e ]B is an eigenvector of [L ]B , then under the similarity transformation(2.128) , the corresponding eigenvector of [L ]

A is [e ]

A = [ A ]

A[e ]

B.

Proof : The characteristic polynomial of [L ]B is

P () = det( [1 ]B [L ]B) (2.129)

J g A g l (M Gill U i it ) Ch t 2 M th ti l B kg d 2009 96 / 122


P () det( [A 1 ]A[1 ]A[A ]A[A 1 ]A[L ]A[A ]A)
http://find/


97/122

A A A A A A= det([ A 1 ]

A([1 ]

A[L ]

A)[ A ]

A)

= det([ A 1 ]A)det( [1 ]A[L ]A)det([ A ]A)

det([ A 1 ]A)det([ A ]A) = 1The characteristic polynomials of [L ]

A and [L ]

B are identical

[L ]

A& [L ]B have same eigenvalues.Let [L ]B[e ]B = [e ]B :

[A 1 ]A[L ]A[A ]A[e ]B = [e ]B[L ]A[A ]A[e ]B = [A ]A[e ]B (2.130)

J A l (M Gill U i it ) Ch t 2 M th ti l B kg d 2009 97 / 122

Similarity Transformations ( Contd )Theorem If [L ]A and [L ]B are related by the similarity transformation (2.127 ) , then
http://find/


98/122

A B[L k ]B = [ A 1 ]A[L k ]A[A ]A (2.131)

for any integer k.

Proof : For k = 2

[L 2 ]B [A 1 ]A[L ]A[A ]A[A

1 ]A[L ]A[A ]A= [ A 1 ]A[L

2 ]A[A ]AFor k = n

[L n +1 ]B [A 1 ]A[L n ]A[A ]A[A 1 ]A[L ]A[A ]A= [ A 1 ]A[L

n +1 ]A[A ]A

J A l (M Gill U i it ) Ch t 2 M th ti l B k d 2009 98 / 122


TheoremThe trace of an n n matrix does not change under a similarity
http://find/


99/122

The trace of an n n matrix does not change under a similaritytransformation.

Proof : Let [A ], [B ] and [C ] be three different n n matrix arrays, in agiven reference frameLet aij , bij , and cij be the components of the said arrays, with indices rangingfrom 1 to n

tr([ A ] [B ] [C ]) aij bjk cki = bjk cki a ij tr([ B ] [C ] [A ])

tr([ L ]B

) = tr([ A 1 ]A

[L ]A

[A ]A

)= tr([ A ]A[A

1 ]A[L ]A) = tr([ L ]A)

J A l (M Gill U i i ) Ch 2 M h i l B k d 2009 99 / 122

Example

Example
http://find/


100/122

ExampleWe consider the equilateral triangle Fig. 2.4 of side 2, with verticesP 1, P 2, P 3, and coordinate frames A(X , Y ) and B(X , Y ), both with originat the centroid of the triangle. Let P be a 2 2 matrix :

P = p1

p2

with p i being the position vector of P i in a given coordinate frame. Show thatmatrix P does not obey a similarity transformation upon a change of frame,and compute its trace in frames Aand B to make it apparent that this matrixdoes not comply with the conditions of Theorem 3.

J A l (M Gill U i i ) Chapter 2: Mathematical Background

2009 100 / 122

Example ( Contd )
http://find/


101/122

F IG .: Two coordinate frames used to represent the position vectors of the corners of an equilateral triangle.


Example ( Contd )

[P ]A = 1 0

3/ 3 2 3/ 3, [P ]B =

0 1

2 3/ 3 3/ 3
http://find/


102/122

tr([ P ]A) = 1 + 2 3

3 = tr([ P ]B) = 33

[p i ]A = [ Q ]A[p i ]B [P ]A = [ Q ]A[P ]BLet R PP T then

[R ]A = 1

3/ 3

3/ 3 5/ 3 , [R ]B = 1 3/ 3 3/ 3 5/ 3

tr([ R ]B) = 83

[R ]A = [ PP T ]A = [ Q ]A[P ]B([ Q ]A[P ]B)T = [ Q ]A[PP ]T B[Q T ]A

which is a similarity transformation


Invariance Concepts

Th l f i f ( ) f h i i i
http://find/


103/122

The scalar function f (

) of the position vector p is

invariant if : f ([p ]B) = f ([p ]A)vector f is invariant if : [f ]A = [Q ]A[f ]Bmatrix F is invariant if : [F ]

A = [Q ]

A[F ]

B[Q T ]

AMoreover,

[a ]T A[b ]A = [ a ]T

B [b ]B[a

b ]

A = [ Q ]

A[a

b ]

B


Invariance Concepts ( Contd )

The kth moment of an n n matrix T :k tr( T k ) k = 0 1
http://find/


104/122

I k

tr( T ), k 0 , 1, . . .

with I 0 = tr( 1 ) = n.TheoremThe moments of a n n matrix are invariant under a similaritytransformation.Proof :

[T k ]B = [ A 1 ]A[T

k ]A[A ]A (2.140)[

I k ]

B = tr( A 1

AT k

A[A ]

A)

tr([ A ]

AA 1

AT k

A)

= tr( T kA

) [ I k ]A



Theorem
http://find/


105/122

An n n matrix has only n linearly independent moments.Proof : Let the characteristic polynomial of T be

P () = a0 + a1 + + an 1 n 1 + n = 0Applying the Cayley-Hamilton Theorem :

a01 + a1T + + an 1T n 1 + T n = 0Take the trace of both sides of the above equation :

a0 I 0 + a1 I 1 + + an 1 I n 1 + I n = 0



The vector invariants of an n n matrix are its eigenvectors.
http://find/


106/122

gThe eigenvectors of symmetric matrices are real and mutually orthogonal, itseigenvalues being real as well.

This is not so for skew-symmetric matrices but for 3 3 skew-symmetricmatrices, one eigenvalue is real, namely, 0, and its associated vector is theaxial vector of this matrix.

Two n n matrices related by a similarity transformation have the same setof moments. However, if two n n symmetric matrices share their rst nmoments {I k }n 10 , they are not necessarily related by a similaritytransformation.



For example :
http://find/


107/122

A = 1 00 1 , B = 1 22 1

Both matrices share the two moments : I 0 = 2 & I 1 = 2 . The secondmoments are different :

tr( A 2) = 2 , tr( B 2) = tr 5 44 5 = 10

To test whether two different matrices represent the same lineartransformation, we must verify that they share the same set of nmoments

{I k

}n1 since all n

n matrices share the same

I 0 = n .



The foregoing discussion does not apply, in general, to nonsymmetric
http://find/


108/122

g g pp y g y

matrices : they are not fully characterized by their eigenvalues. For example :

A = 1 10 1

Moments I 0 = 2 , I 1 = tr( A ) = 2 are equal to those of the 2 2 identitymatrix but the transformations are different.If two symmetric matrices, A and B , represent the same transformation, theyare related by a similarity transformation :

B = T 1AT


Examples
http://find/


109/122

ExampleFind out whether two symmetric matrices

A =1 0 10 1 0

1 0 2

, B =1 0 00 2 10 1 1

are related by a similarity transformation.

tr( A ) = tr( B ) = 4 . What about I 2 and I 3 ?


Examples ( Contd )
http://find/


110/122

A 2 = 2 0 30 1 03 0 5

, B 2 = 1 0 00 5 30 3 2tr( A 2) = tr( B 2) = 8

A 3 =5 0 80 1 08 0 13

, B 3 =1 0 00 13 80 8 5

tr( A 3) = tr( B 3) = 19


Examples ( Contd )

Example
http://find/


111/122

ExampleSame as the previous example, for :

A =1 0 20 1 0

2 0 0

, B =1 1 11 1 0

1 0 0

tr( A ) = tr( B ) = 2 , but tr( A 2) = 10 while tr( B 2) = 6

Therefore, A and B are not related by a similarity transformation


Applications to Redundant Sensing
http://find/


112/122

If redundant sensors are introduced, and we attach frames Aand B to each of these, then each sensor can be used to determine the orientation of theend-effect- or with respect to a reference conguration.

For this task it is needed to measure rotation R that each frame underwentfrom the reference conguration.

Assume that the measurements produce the orthogonal matrices A and B ,representing R in Aand B, respectively. Determine the relative orientation Qof frame B with respect to frame A(instrument calibration ).


Applications to Redundant Sensing ( Contd )
http://find/


113/122

We have : A [R ]A and B [R ]BNeed to determine [Q ]A or [Q ]B, which can be obtained from

A = [ Q ]AB [QT ]A or A [Q ]A = [ Q ]AB

This problem can be solved with three invariant vectors associated with A andB .Since A and B are orthogonal, they admit one real invariant vector : their axialvector .



To determine Q , we need two more invariant vectors, represented in and .
http://find/


114/122

p

A BTo this end, take two measurements of the orientation of A0 and B0 withrespect to Aand B.Let the matrices representing these orientations be given by A i and B i , witha i and b i being the corresponding axial vectors, for i = 1 , 2.

There are two possibilities :

(i) Neither of a 1 and a 2 and, consequently, neither of b 1 and b 2, is zero ;(ii ) at least one of a 1 and a 2, and consequently, the corresponding vector of

the

{b 1, b 2

}pair, vanishes.


http://find/


115/122

In the rst case compute a 3rd vector for each seta 3 = a 1 a 2, b 3 = b 1 b 2 (2.143)

In the second case, two more possibilities : angle of rotation of orthogonalmatrix, A

1 or A

2, whose axial vector vanishes, is either

0 or

.

If angle is 0, then A, as well as B, underwent a pure translation from A0 andB0, correspondingly. Then, a new measurement is needed, involving arotation.



If the angle is , then associated rotation is symmetric and the unit vector eparallel to its axis can be determined from eq.( 2.49 ) in both Aand B.

2 2
http://find/


116/122

Then, we end up with two pairs of nonzero vectors {a i }21 and {b i }

21.

a i = [ Q ]Ab i , for i = 1 , 2, 3 (2.144)

E = [ Q ]AF (2.145)

E a 1 a 2 a 3 , F b 1 b 2 b 3 (2.146)

[Q ]A = EF 1 (2.147)




117/122

F 1 = 1

(b 2 b 3)T (b 3 b 1)T (b 1 b 2)T , b 1 b 2 b 3 (2.148)

Therefore,

[Q ]A = 1

[a 1(b 2 b 3)T + a 2(b 3 b 1)T + a 3(b 1 b 2)T ]


Example


118/122

Example (Hand-Eye Calibration)Determine the relative orientation of a frame B attached to a camera mounted on arobot end-effector, with respect to a frame Axed to the latter, as shown in Fig. 2.5 .It is assumed that two measurements of the orientation of the two frames with respect

to frames A0 and B0 in the reference conguration of the end-effector are available.These measurements produce the orientation matrices A i of the frame xed to thecamera and B i of the frame xed to the end-effector, for i = 1 , 2.


Example ( Contd )

The numerical data :

A 0 .92592593 0 .37037037 0 .07407407

0 28148148 0 80740741 0 51851852


119/122

A1 = . . .0 .25185185 0 .45925926 0 .85185185

A 2 =0 .83134406 0 .02335236 0 .555267250 .52153607 0 .31240270 0 .793980280 .19200830 0 .94969269 0 .24753503

B 1 =0 .90268482 0 .10343126 0 .41768659

0 .38511568 0 .62720266 0 .676980600 .19195318 0 .77195777 0 .60599932

B 2 =0 .73851280 0 .54317226 0 .399453050 .45524951 0 .83872293 0 .298817210 .49733966 0 .03882952 0 .86668653


Example ( Contd )


120/122

F IG .: Measuring the orientation of a camera-xed coordinate frame with respect to aframe xed to a robotic end-effector.


Example ( Contd )

0 02962963 0 04748859
http://find/


121/122

a 1 = .0 .088888890 .32592593

, b 1 = .0 .304819890 .14084221,

a 2 =0 .07784121

0 .373637780 .27244422

, b 2 =0 .12999385

0 .448696360 .04396138

a 3 =0 .097560970 .017302930 .00415020

, b 3 =0 .076553430 .016220960 .06091842

= 0 .00983460


Example ( Contd )

T 0 .00078822 0 .00033435 0 .00107955


122/122

a 1(b 2 b 3) = 0 .00236467 0 .00100306 0 .003238660 .00867044 0 .00367788 0 .01187508

a 2(b 3 b 1)T =0 .00162359 0 .00106467 0 .001756800 .00779175 0 .00510945 0 .00843102

0 .00568148 0 .00372564 0 .00614762

a 3(b 1 b 2)T = 0 .00746863 0 .00158253 0 .005943260 .00132460 0 .00028067 0 .00105407

0 .00031771 0 .00006732 0 .00025282

[Q ]A =0 .84436553 0 .01865909 0 .535457500

.41714750

0

.65007032

0

.63514856

0 .33622873 0 .75964911 0 .55667078

http://find/

Present Ac i on Cap i Tulo 2 Angeles

Documents

Transcript of Present Ac i on Cap i Tulo 2 Angeles