Spatial Vector Algebra

Transcription

1 A Short Course on The Easy Way to do Rigid Body Dynamics Roy Featherstone Dept. Inormation Engineering, RSISE The Australian National University Spatial vector algebra is a concise vector notation or describing rigid body velocity, acceleration, inertia, etc., using 6D vectors and tensors. ewer quantities ewer equations less eort ewer mistakes Roy Featherstone 2 Mathematical Structure spatial vectors inhabit two vector spaces: M 6 F 6 motion vectors orce vectors with a scalar product deined between them m. = work. : M 6 F 6 R Bases A coordinate vector m=[m 1,.,m 6 ] T represents a motion vector m in a basis {d 1,.,d 6 } on M 6 i 6 m = i=1 Σ mi d i Likewise, a coordinate vector =[ 1,., 6 ] T represents a orce vector in a basis {e 1,.,e 6 } on F 6 i 6 = Σ i e i i=1 3 4

2 Bases v I {d 1,., d 6 } is an arbitrary basis on M 6 then there exists a unique reciprocal basis {e 1,., e 6 } on F 6 satisying The velocity o a rigid body can be described by d i. e j = { 0 : i = j 1 : i = j With these bases, the scalar product o two coordinate vectors is m. = m T choosing a point,, in the body speciying the linear velocity, v, o that point, and speciying the angular velocity,, o the body as a whole 5 6 v v The body is then deemed to be translating with a linear velocity v while simultaneously rotating with an angular velocity about an axis passing through Now introduce a coordinate rame with an origin at any ixed point 7 8

3 v v v v 9 Deine v to be the velocity o the body ixed point that coincides with at the current instant v = v 10 The body can now be regarded as translating with a velocity o v while simultaneously rotating with an angular velocity o about an axis passing through Introduce the unit vectors i, j and k pointing in the x, y and z directions. and v can now be expressed in terms o their Cartesian coordinates: = x y z v = v x v y v z x z i k = x i y j z k v = v x i v y j v z k j y The motion o the body can now be expressed as the sum o six elementary motions: a linear velocity o v x in the x direction a linear velocity o v y in the y direction a linear velocity o v z in the z direction an angular velocity o x about the line x an angular velocity o y about the line y an angular velocity o z about the line z 11 coordinate vectors what they represent 12

4 13 Deine the ollowing lucker basis on M 6 : x d x d x z d z d y d x unit angular motion about the line x d y unit angular motion about the line y d z unit angular motion about the line z d x d y d z unit linear motion in the x direction unit linear motion in the y direction unit linear motion in the z direction d z d y y 14 v v The spatial velocity o the body can now be expressed as = v v = x d x y d y z d z v x d x v y d y v z d z v v v = v v = v 15 This single quantity provides a complete description o the velocity o a rigid body, and it is invariant with respect to the location o the coordinate rame 16 The six scalars x, y,., v z are the lucker coordinates o v in the coordinate system deined by the rame xyz

5 v v = v 17 x y v z = x d x y d y v = = v v x z d z v x d x v y v y d y v z d z v z coordinate vector what it represents 18 Now try question set A Force A general orce acting on a rigid body can be expressed as the sum o a linear orce acting along a line passing through any chosen point, and n Force I we choose a dierent point,, then the orce can be expressed as the sum o a linear orce acting along a line passing through the new point, and n n a couple, n a couple n, where n = n 19 20

6 21 Force Now place a coordinate rame at and introduce unit vectors i, j and k, as beore, so that n = n x i n y j n z k = x i y j z k n x n = n y = n z n n x y z 22 The total orce acting on the body can now be expressed as the sum o six elementary orces: a moment o n x in the x direction a moment o n y in the y direction a moment o n z in the z direction a linear orce o x acting along the line x a linear orce o y acting along the line y a linear orce o z acting along the line z 23 Deine the ollowing lucker basis on F 6 : e x e y e z e x e y e z e x e x e z x unit couple in the x direction unit couple in the y direction unit couple in the z direction unit linear orce along the line x unit linear orce along the line y unit linear orce along the line z z y e z e y e y 24 Force The spatial orce acting on the body can now be expressed as = n x e x n y e y n z e z x e x y e y z e z n n This single quantity provides a complete description o the orces acting on the body, and it is invariant with respect to the location o the coordinate rame =

7 25 Force The six scalars n x, n y,., z are the lucker coordinates o in the coordinate system deined by the rame xyz coordinate vector: = n = n x n y n z x y z n = n 26 lucker Coordinates lucker coordinates are the standard coordinate system or spatial vectors a lucker coordinate system is deined by the position and orientation o a single Cartesian rame a lucker coordinate system has a total o twelve basis vectors, and covers both vector spaces (M 6 and F 6 ) 27 lucker Coordinates the lucker basis e x, e y,., e z on F 6 is reciprocal to d x, d y,., d z on M 6 so the scalar product between a motion vector and a orce vector can be expressed in lucker coordinates as v. T = v which is invariant with respect to the location o the coordinate rame 28 Coordinate Transorms transorm rom A to B or motion vectors: B X A = E 0 0 E 1 0 ~ r T 1 corresponding transorm or orce vectors: where A ~ r = r 0 r z r y r z 0 r x B X A * = ( B X A ) T B E r y r x 0

8 Basic perations with Spatial Vectors Relative velocity I bodies A and B have velocities o v A and v B, then the relative velocity o B with respect to A is v rel = v B v A Rigid Connection I two bodies are rigidly connected then their velocities are the same Summation o Forces I orces 1 and 2 both act on the same body, then they are equivalent to a single orce tot given by tot = 1 2 Action and Reaction I body A exerts a orce on body B, then body B exerts a orce on body A (Newton s 3rd law) Scalar roduct I a orce acts on a body with velocity v, then the power delivered by that orce is power = v. Scalar Multiples A velocity o αv causes the same movement in 1 second as a velocity o v in α seconds. A orce o β delivers β times as much power as a orce o Now try question set B 31 32

9 Spatial Cross roducts Dierentiation There are two cross product operations: one or motion vectors and one or orces v m = v m m = m m v m The derivative o a spatial vector is itsel a spatial vector in general, d s = lim dt δt 0 s(tδt) s(t) δt v * = v * n = n v The derivative o a spatial vector that is ixed in a body moving with velocity v is 33 where v and m are motion vectors, and is a orce. 34 d dt s = v s v * s i s M 6 i s F 6 Dierentiation in Moving Coordinates Acceleration d dt s = d dt s v s or * i s F 6 velocity o coordinate rame... is the rate o change o velocity: d v = dt. a =. v componentwise derivative o coordinate vector s but this is not the linear acceleration o any point in the body! 35 coordinate vector representing ds/dt 36

10 Acceleration Acceleration Example 37 is a ixed point in space, and v (t) is the velocity o the body ixed point that coincides with at time t, so v is the velocity at which body ixed points are streaming through.. v is thereore the rate o change o stream velocity 38 v ixed axis I a body rotates with constant angular velocity about a ixed axis, then its spatial velocity is constant and its spatial acceleration is zero; but each body ixed point is ollowing a circular path, and is thereore accelerating. Acceleration Formula Basic roperties o Acceleration 39 Let r be the 3D vector giving the position o the body ixed point that coincides with at the current instant, measured relative to any ixed point in space we then have but. a =. = v v = = v.. r r ṙ 40 Acceleration is the time derivative o velocity Acceleration is a true vector, and has the same general algebraic properties as velocity Acceleration ormulae are the derivatives o velocity ormulae I v tot = v 1 v 2 then a tot = a 1 a 2 (Look, no Coriolis term!)

11 Rigid Body Inertia mass: m Now try question set C c C CoM: inertia at CoM: C I C I spatial inertia tensor: I = m ~ c m ~ c T m1 where I = I C m ~~ c c Basic perations with Inertias Composition I two bodies with inertias I A and I B are joined together then the inertia o the composite body is I tot = I A I B Coordinate transormation ormula I B = B X A * I A A X B = ( A X B ) T I A A X B 44 Equation o Motion d = (I v) = I a v * I v dt = net orce acting on a rigid body I = inertia o rigid body v = velocity o rigid body I v = momentum o rigid body a = acceleration o rigid body

12 Motion Constraints I a rigid body s motion is constrained, then its velocity is an element o a subspace, S M 6, called the motion subspace degree o (motion) reedom: degree o constraint: S can vary with time dim(s) 6 dim(s) Motion Constraints Motion constraints are caused by constraint orces, which have the ollowing property: A constraint orce does no work against any motion allowed by the motion constraint (D Alembert s principle o virtual work, and Jourdain s principle o virtual power) Motion Constraints Constraint orces are thereore elements o a constraint orce subspace, T F 6, deined as ollows: T = {. v = 0 v S } This subspace has the property Matrix Representation The subspace S can be represented by any 6 dim(s) matrix S satisying range(s) = S Likewise, the subspace T can be represented by any 6 dim(t) matrix T satisying range(t) = T dim(t) = 6 dim(s) 47 48

13 roperties any vectors v S and T can be expressed as v = S α and = T λ, where α and λ are dim(s) 1 and dim(t) 1 coordinate vectors S T T = 0, which implies... S T = 0 and T T v = 0 or all T and v S Constrained Motion Analysis An Example: A orce,, is applied to a body that is constrained to move in a subspace S = range(s) o M 6. The body has an inertia o I, and it is initially at rest. What is its acceleration? c I a relevant equations: v = S α.. a = S α S α S T c = 0 c = I a v * I v v = 0 implies α = 0 a = S α. c = I a I c a solution: c = I S α. S T = S T I S α.. α = (S T I S) 1 S T a = S (S T I S) 1 S T 52 Now try question set D

14 Inverse Dynamics joint 1 joint 2 joint n velocity o link i is the velocity o link i 1 plus the velocity across joint i v i = v i 1 s i q i. base. q i, q i, s i v i, a i i τ i link 1 link velocity and acceleration link n joint velocity, acceleration & axis orce transmitted rom link i 1 to i joint orce variable acceleration is the derivative o velocity equation o motion active joint orce.. a i = a i 1 s i q i s i q i i i 1 = I i a i v i * I i v i 53 I i link inertia 54 τ i = s i T i The Recursive Newton Euler Algorithm (Calculate the joint torques τ i that will produce the desired joint accelerations q i.). v i = v i 1 s i q i.. a i = a i 1 s i q i s i q i i = i 1 I i a i v i * I i v i (v 0 = 0) (a 0 = 0) ( n1 = ee ) τ i = s i T i 55