Six-Degree-of-Freedom Kinematics and Dynamics of a Rigid Body

Transcription

1 Six-Degree-of-Freedom Kinematics and Dynamics of a Rigid Body I. Kinematics Consider an inertial frame X i Y i Z i with origin O i and a body-fixed frame X b Y b Z b with origin O b. The rotation matrix R b i which transforms vectors in body-fixed frame to the inertial frame can be parametrized in terms of three angles θ x, θ y, and θ z as R b i = R z,θz R y,θy R x,θx (1) where R x,θx denotes the rotation matrix corresponding to a rotation about X-axis by the angle θ x, etc. Hence, R b i = = c z s z 0 s z c z c y 0 s y s y 0 c y c y c z s x s y c z c x s z c x s y c z + s x s z c y s z s x s y s z + c x c z c x s y s z s x c z s y s x c y c x c y c x s x 0 s x c x (2) where c x = cos(θ x ), s x = sin(θ x ), etc. The position and orientation of the body relative to the inertial frame can be represented by a 6 1 vector p = p T t, p T r T where p t = x, y, z T represents the Cartesian coordinates of O b as measured in the inertial frame and p r = θ x, θ y, θ z T specifies the rotation angles. The translational and angular velocities of the body relative to the inertial frame and expressed in the body-fixed frame are denoted as v t and v r, respectively. Let v = vt T, vr T T. Hence, ṗ t = R b iv t = J t (p r )v t ; J t (p r ) = R b i. (3) By definition, the angular velocity v r satisfies 1 Ṙ b i = R b is(v r ) (4) where S(ω) with ω = ω x, ω y, ω z T denotes the skew-symmetric matrix S(ω) = 0 ω z ω y ω z 0 ω x ω y ω x 0. (5) 1 Note that v r is the angular velocity expressed in the body-fixed frame. The angular velocity in the inertial frame is R b i v r. Hence, Ṙ b i = S(Rb i v r)r b i = Rb i S(v r)r b i T R b i = R b i S(v r). 1

2 Denoting by e 1, e 2, and e 3 the three principal unit vectors, i.e., e 1 = 1, 0, 0 T, e 2 = 0, 1, 0 T, and e 3 = 0, 0, 1 T, we have Ṙ x,θx = θ x R x,θx S(e 1 ) Ṙ y,θy = θ y R y,θy S(e 2 ) Ṙ z,θz = θ z R z,θz S(e 3 ). (6) Hence, Ṙi b = R z,θz R y,θy R x,θx θx S(e 1 ) + R z,θz R y,θy θy S(e 2 )R x,θx + R z,θz θz S(e 3 )R y,θy R x,θx = R z,θz R y,θy R x,θx θ x S(e 1 ) + Rx,θ T x θy S(e 2 )R x,θx + Rx,θ T x θz S(e 3 )R y,θy R x,θx = Ri b θ x S(e 1 ) + θ y S(Rx,θ T x e 2 ) + θ z S(Rx,θ T x e 3 ) = RiS( b θ x e 1 + θ y Rx,θ T x e 2 + θ z Rx,θ T x e 3 ). (7) Comparing with (4), v r is related to ṗ r as v r = θ x e 1 + θ y R T x,θ x e 2 + θ z R T x,θ x R T y,θ y e 3 = v r = e 1.Rx,θ T x e 2.Rx,θ T x e 3 ṗ r 1 0 s y = 0 c x s x c y ṗ r. (8) 0 s x c x c y Hence, ṗ r = J r (p r )v r (9) where J r (p r ) = 1 s x t y c x t y 0 c x s x 0 s x /c y c x /c y (10) where t x = tan(θ x ) and t y = tan(θ y ). Using (3) and (9), ṗ = J(p r )v (11) where 2 J(p r ) = Jt (p r ) J r (p r ) 2 I n n and 0 n n denote the n n identity matrix and zero matrix, respectively. 2

3 = c y c z s x s y c z c x s z c x s y c z + s x s z c y s z s x s y s z + c x c z c x s y s z s x c z s y s x c y c x c y s x t y c x t y c x s x s x /c y c x /c y. (12) II. Dynamics The translational dynamics of a rigid body are obtained from the observation that the mass of the body times the acceleration of the center of mass is equal to the net force acting on the body. Denote by G the center of mass of the rigid body. Let p G,i and p G,b denote the coordinates of G relative to the inertial and body-fixed frames, respectively. Then, p G,i = p t + R b ip G,b. (13) Differentiating (13) and using (3) and (4) yields ṗ G,i = R b iv t + R b is(v r )p G,b (14) = p G,i = R b i v t + S( v r )p G,b + R b is(v r )v t + S(v r )p G,b. (15) Denoting the mass of the body by m and the net force acting on the body expressed in the body-fixed frame by F, the translational dynamics of the rigid body are given by m p G,i = R b if. (16) Using (15) and (16), the translational dynamics can be written in terms of the translational and angular velocities expressed in the body-fixed frame as m v t + S( v r )p G,b + S(v r )v t + S(v r )S(v r )p G,b = F. (17) Using the property of skew-symmetric matrices that 3 S(a)b = a b = b a = S(b)a, (17) can be rewritten as m v t S(p G,b ) v r + S(v r )v t S(v r )S(p G,b )v r = F. (18) To facilitate the derivation of the rotational dynamics, consider a decomposition of the rigid body into infinitesimal volume elements dv with local densities ρ dv. Let p dv,i and p dv,b denote the coordinates of the infinitesimal element dv relative to the inertial and body-fixed frames, 3 a b with a and b being vectors denotes the vector cross-product of a and b. 3

4 respectively, and let v dv,i denote the velocity of dv expressed in the inertial frame. Note that each p dv,b is constant by the definition of a rigid body. The net torque about the point O b acting on the rigid body is given in the inertial frame by τ i = (p dv,i p t ) v dv,i ρ dv dv (19) where the integral is carried out over all the volume elements composing the rigid body. Hence, the net torque expressed in the body-fixed frame is τ = R b i T τi = R b i T (p dv,i p t ) v dv,i ρ dv dv. (20) Using the relations p dv,i = p t + Rip b dv,b (21) v dv,i = Riv b t + S(v r )p dv,b (22) v dv,i = Ri b v t + S( v r )p dv,b + S(v r )v t + S(v r )S(v r )p dv,b (23) (p dv,i p t ) v dv,i = Ri{p b dv,b v t + S( v r )p dv,b + S(v r )v t + S(v r )S(v r )p dv,b }, (24) (20) can be expanded as τ = {p dv,b v t + S( v r )p dv,b + S(v r )v t + S(v r )S(v r )p dv,b }ρ dv dv. (25) The mass m and the moment of inertia I b (about the point O b ) satisfy, by definition, mp G,b = p dv,b ρ dv dv (26) I b v r = {p dv,b S( v r )p dv,b }ρ dv dv (27) I b = S(p dv,b )S(p dv,b )ρ dv dv. (28) Note that I b is a constant symmetric positive-definite matrix. Using the vector triple product identity given by a (b c) + b (c a) + c (a b) = 0 (29) valid for any vectors a, b, and c, we obtain p dv,b S(v r )S(v r )p dv,b = S(p dv,b )S(v r )v r p dv,b = S(v r )S(v r p dv,b )p dv,b S(v r p dv,b )S(p dv,b )v r = S(v r )S(v r p dv,b )p dv,b S(v r p dv,b )(p dv,b v r ) = S(v r )S(v r p dv,b )p dv,b = S(v r )S(p dv,b )(v r p dv,b ) 4

5 Hence, = S(v r )S(p dv,b )S(p dv,b )v r. (30) {p dv,b S(v r )S(v r )p dv,b }ρ dv dv = S(v r )I b v r = S(I b v r )v r. (31) Using (26), (27), (28), and (31), (25) reduces to τ = ms(p G,b ) v t + I b v r + ms(p G,b )S(v r )v t S(I b v r )v r. (32) The rigid body kinematics given by (11) and the translational and rotation dynamics given by (18) and (32) can be rewritten as ṗ = J(p)v M RB v + C RB (v)v = F (33) (34) where M RB = C RB (v) = mi 3 3 ms(p G,b ) ms(p G,b ) I b ms(v r ) ms(v r )S(p G,b ) ms(p G,b )S(v r ) S(I b v r ) (35) (36) and F is the 6 1 generalized force vector given by F = F T, τ T T. Note that M RB is a constant symmetric positive-definite matrix while C RB (v) is a skew-symmetric matrix that depends on the angular velocity of the rigid body. Introducing the notations p G,b = p G,b,x, p G,b,y, p G,b,z T v t = v t,x, v t,y, v t,z T v r = v r,x, v r,y, v r,z T F = F x, F y, F z T τ = τ x, τ y, τ z T I b = I b,xx I b,xy I b,xz I b,xy I b,yy I b,yz I b,xz I b,yz I b,zz (37) (38) (39) (40) (41), (42) 5

6 M RB and C RB (v) are obtained as M RB = m mp G,b,z m G,b,y 0 m 0 mp G,b,z 0 mp G,b,x 0 0 m mp G,b,y mp G,b,x 0 0 mp G,b,z m G,b,y I b,xx I b,xy I b,xz mp G,b,z 0 mp G,b,x I b,xy I b,yy I b,yz (43) C RB (v) = mp G,b,y mp G,b,x 0 I b,xz I b,yz I b,zz C11 (v) C 12 (v) C 21 (v) C 22 (v) (44) where 0 mv r,z mv r,y C 11 (v) = mv r,z 0 mv r,x mv r,y mv r,x 0 vr,zp G,b,z + v r,yp G,b,y v r,yp G,b,x v r,zp G,b,x C 12 (v) = v r,xp G,b,y v r,zp G,b,z + v r,xp G,b,x v r,zp G,b,y v r,xp G,b,z v r,yp G,b,z v r,yp G,b,y + v r,xp G,b,x vr,zp G,b,z v r,yp G,b,y v r,xp G,b,y v r,xp G,b,z C 21 (v) = C 22 (v) = v r,yp G,b,x v r,zp G,b,z v r,xp G,b,x v r,yp G,b,z v r,zp G,b,x v r,zp G,b,y v r,yp G,b,y v r,xp G,b,x 0 (I b,xz v r,x + I b,yz v r,y + I b,zz v r,z) (I b,xy v r,x + I b,yy v r,y + I b,yz v r,z) (I b,xz v r,x + I b,yz v r,y + I b,zz v r,z) 0 (I b,xx v r,x + I b,xy v r,y + I b,xz v r,z) (I b,xy v r,x + I b,yy v r,y + I b,yz v r,z) (I b,xx v r,x + I b,xy v r,y + I b,xz v r,z) 0 (45) (46) (47).(48) 6