Nemanja D. Zoric Research Assistant University of Belgrade Faculty of Mechanical Engineering
Mihailo P. Lazarevic Full Professor University of Belgrade Faculty of Mechanical Engineering
Aleksandar M. Simonovic Assistant Professor University of Belgrade Faculty of Mechanical Engineering
Multi-Body Kinematics and Dynamics in Terms of Quaternions: Langrange Formulation in Covariant Form – Rodriguez Approach This paper suggests a quaternion approach for the modelling kinematics and dynamics of rigid multi-body systems. Instead of the regular “Newton-Euler” and Lagrange method used in the traditional way, Lagrange’s equations of second kind in the covariant form are used by applying Rodriguez approach and quaternion algebra. A model of multi-body system of n rigid bodies in terms of quaternions is obtained, which is useful for studying kinematics, dynamics as well as for research of control system designs. Keywords: quaternions, rotation, rigid bodies system.
1. INTRODUCTION
In general, modelling of kinematics and dynamics of rigid bodies systems has been mostly based on the Euler angles representation of rotation. It is well known that three angles can’t afford a regular representation of the rotation, since there are singularities. Euler proposed a solution to circumvent this problem by introducing a set of four quantities, the so-called Euler parameters, based on relations among Euler angles. Later on, Hamilton (1844) invented the quaternions, an extension of complex numbers, and soon afterwards, it was discovered that rotations may be represented by quaternions [1]. In [2] and [3], Lagrange’s equations of second kind of rigid bodies system in covariant form were developed using Rodriguez matrix for the representation of orientation of rigid body with respect to the inertial frame. Our goal is to develop the same form of equation, but with the help of quaternions. Further research will be based on control system designs, because quaternions enable singularity-free mathematical representation of orientations [4,5]. 2. MATHEMATICAL BACKGROUND OF THE QUATERNIONS
Quaternions are hyper-complex numbers of rank 4 consisting of one real and three imaginary parts. The quaternions were first described by Irish mathematician Sir William Rowan Hamilton in 1844 and applied to mechanics in three-dimensional space. Crucial to this description was his celebrated rule: (1)
where ⊗ denotes quaternion or the Hamilton product. The quaternion is defined as: Received: December 2009, Accepted: January 2010 Correspondence to: Nemanja Zoric, M.Sc. Faculty of Mechanical Engineering, Kraljice Marije 16, 11120 Belgrade 35, Serbia E-mail:
[email protected] © Faculty of Mechanical Engineering, Belgrade. All rights reserved
(2)
where q0 represents real part, and q1, q2 and q3 represent imaginary parts of the quaternion. Pure part of the quaternion (2) is defined as: q = q1i + q2 j + q3k .
(3)
2.2 Algebraic properties
Let p and q be two quaternions. The sum of p and q can be written as: p + q = ( p0 + q0 ) + ( p1 + q1 ) i + ( p2 + q2 ) j + ( p3 + q3 ) k . (4)
If c is scalar, then the product of quaternion q and scalar c is given by: cq = cq0 + cq1i + cq2 j + cq3k .
(5)
For two quaternions p and q, their Hamilton or quaternion product is determined by the product of the basis elements and the distributive law. This gives the following expression [6]: p ⊗ q = p0 q0 − p ⋅ q + p0 q + q0p + p × q .
2.1 Definition
i 2 = j2 = k 2 = i ⊗ j ⊗ k = -1 ,
q = q0 + q1i + q2 j + q3k ,
(6)
From (6) it can be seen that quaternions form noncommutative under multiplication. Let q be the quaternion. The complex conjugate of quaternion q is defined as: q* = q0 − q = q0 − q1i − q2 j − q3k .
(7)
From (6) and (7) it can be concluded the following:
( p ⊗ q )* = q* ⊗ p* .
(8)
The norm of quaternion q is defined as: N ( q ) = q* ⊗ q = q02 + q12 + q22 + q32 = q ,
(9)
and inverse of quaternion q is: q −1 =
q* N 2 (q)
=
q* q
2
.
FME Transactions (2010) 38, 19-28
(10)
19
2.3 Quaternionic representation of rotation of the rigid body
The rigid body (V) rotates about axis 0τ which is represented by unit vector e (Fig. 1). Reference frame 0xyz is inertial, and reference frame 0ξηζ is body-fixed frame. Unit vectors of axis x, y and z are denoted by i, j and k, and unit vectors of axis ξ, η and ζ are denoted by λ, µ and ν. Figure 2. Quaternionic representation of rotation of the rigid body
Vector e is invariant, and because of that property it is all the same in which the coordinate frame this vector would be expressed. The next case is when the rigid body rotates about the moving axis. The rigid body (V) rotates about moving axis 0τ2 represented by unit vector e2. At the same time, this axis rotates about the axis 0τ1 represented by unit vector e1. The reference frame 0xyz is inertial, 0ξ2η2ζ2 is body-fixed and 0ξ1η1ζ1 is fixed on the rotation axis 0τ2 (Fig. 3). At initial time these three frames were equivalent. Figure 1. Rotation of the rigid body
In initial time these two reference frames were equivalent. Vector OM = r (point M belongs to the body (V)) can be expressed in both reference frames: r = xi + yj + zk ,
(11)
1 r( ) = ξλ + η µ + ζ ν .
(12)
and
Superscript in (12) denotes body-fixed reference frame in which vector r is expressed. Vectors (11) and (12) belong to the set of vectors. In order to operate with the quaternion, a vector, which lives in R3, needs to be treated as a pure quaternion (that is a quaternion which real part is zero) which lives in R4. The set of all pure quaternions (denoted by Q0) is the subset of Q, the set of all quaternions. It can be defined as one-to-one correspondence between the set of vectors and the set of pure quaternions, a correspondence in which a vector r ∈ R 3 corresponds to pure quaternion r = 0 + r ∈ Q0 , that is: r ∈ R3 → r = 0 + r ∈ Q0 ⊂ Q .
(13)
The relation between the vector expressed in inertial frame (11) and the vector expressed in body-fixed frame (12) is given by: 1 r = q ⊗ r( ) ⊗ q* ,
(14)
where q is unit quaternion (a quaternion with norm one) which has the following structure: q = cos
θ 2
+ e sin
θ 2
,
(15)
where θ represents the rotation angle about axis 0τ, and e is pure quaternion which corresponds to the unit vector axis 0τ. The result of (14) is also pure quaternion which corresponds to the position vector of point M. It can be illustrated in the following figure (Fig. 2): 20 ▪ VOL. 38, No 1, 2010
Figure 3. Rotation of the rigid body about moving axis
Vector OM = r (point M belongs to the body (V)) expressed in reference frame 0ξ1η1ζ1 is: 1 2 r( ) = q2 ⊗ r( ) ⊗ q2* ,
(16)
θ ( 2) θ q2 = cos 2 + e 2 sin 2 . 2 2
(17)
where
It must be mentioned that vector e2 is invariant in relation to reference frames 0ξ1η1ζ1 and 0ξ2η2ζ2, and vector e1 is invariant in relation to reference frames 0xyz and 0ξ1η1ζ1. Vector OM = r expressed in inertial reference frame is: 1 r = q1 ⊗ r ( ) ⊗ q1* ,
(18)
θ θ q1 = cos 1 + e1 sin 1 . 2 2
(19)
where
Substituting (16) in (18), one can get following expression: FME Transactions
2 r = q1 ⊗ q2 ⊗ r( ) ⊗ q2* ⊗ q1* = * 2 2 = ( q1 ⊗ q2 ) ⊗ r ( ) ⊗ ( q1 ⊗ q2 ) = q ⊗ r ( ) ⊗ q* , (20)
the axis ej for angle qj, vector τj in body-fixed reference frame 0ξj-1ηj-1ζj-1 has the following value:
( j −1) = p ⊗ τ ( j ) ⊗ p* , τj j j j
where q = q1 ⊗ q2
(21)
where pj represents unit quaternion which is defined as
represents composite quaternion.
p j = cos
3. KINEMATICS OF OPEN CHAIN SYSTEM
(25)
qj qj ( j) + e j sin . 2 2
(26)
If rigid body (Vj-2) is connected with (Vj-1) by cylindrical joint, then:
3.1 Transformation of coordinates
The open chain system of rigid bodies (V1), (V2), ..., (Vn) is shown in Figure 4. The rigid body (V1) is connected to the fixed stand. Two neighboring bodies, (Vi-1) and (Vi) of chain are connected together with joint (i), which allows translation along the axis which is represented by unit vector ei, or rotation about the same axis body (Vi) in respect to body (Vi-1). The values qi represent generalized coordinates.
( j −2) = p ( j) * τj j − 2, j ⊗ τ j ⊗ p2 −1, j ,
(27)
p j − 2, j = p j −1 ⊗ p j ,
(28)
where
and: p j −1 = cos
q j −1 ( j −1) q j −1 . + e j −1 sin 2 2
(29)
In the case of prismatic joint, vector τj is the same in both local body-fixed frames:
( j −1) = τ ( j ) . τj j Figure 4. Open chain of the rigid bodies system
The reference frame 0xyz is inertial Cartesian frame, and the reference frame 0ξiηiζi is local body-frame which is associated to the body (Vi) at the point Ci which represents the centre of inertia of body (Vi). At initial time, corresponding axis of reference frames were parallel. This configuration is called reference configuration and it is denoted by (0). The symbols ξi and ξ can be introduced, which are defined as:
ξi = 1 , ξi = 0
(22)
In the general case, with the help of symbol ξ , the quaternion (26) has the following form: p j = cos
(23)
in the case when bodies (Vi-1) and (Vi) are connected with cylindrical joint. Arbitrary vector τj, associated with the body (Vj) is given (Fig. 5). In reference configuration, this vector is identical in both reference frames
( j) ( j −1) τj 0 =τj 0 . ( ) ( )
(24)
ξ jq j 2
j
ξ jq ( j) + e j sin . 2
(31)
Vector τj in inertial reference frame 0xyz has the following value:
( j) * , τ j = p0, j ⊗ τ j p0, j
(32)
p0, j = p1 ⊗ p2 ⊗ ... ⊗ p j ,
(33)
p j, j = 1 .
(34)
where
in the case when bodies (Vi-1) and (Vi) are connected with prismatic joint, and
ξi = 0 , ξi = 1
(30)
and
3.2 Velocity of inertia centre of the rigid body (Vi)
The position vector of inertia centre Ci of the rigid body (Vi) has the following value in inertial reference frame (Fig. 4): OCi = ri =
i
∑ ( ρkk + ξk ek q k ) + ρi ,
(35)
k =1
where
Figure 5. Vector τj on rigid body (Vj)
In the case when bodies (Vj-1) and (Vj) are connected by cylindrical joint, after rotation of the body (Vj) about FME Transactions
(k ) * ρkk = p0,k ⊗ ρkk ⊗ p0, k,
(36)
(k ) * ek = p0,k −1 ⊗ ek ⊗ p0, k −1 ,
(37)
(i ) * ρi = p0,i ⊗ ρi ⊗ p0, i.
(38)
VOL. 38, No 1, 2010 ▪ 21
Velocity of the centre of inertia is: i
∂pα*
α
i
dr ∂r Vi = i = ∑ i qα = ∑ Tα ( i ) qα , dt α =1 ∂qα α =1
(39)
Tα ( i ) =
∂qα
,
∂ρkk
(40)
α
∂q
which is called the quasi-basic vector. Generalized coordinate partial derivatives of the quaternion are: ∂pk
∂qα
= 0, k ≠α ,
(41)
=
∂q
ξ qα 1 ξ qα 1 α = − ξα sin α + ξα eα( ) cos α = 2 2 2 2 ∂qα ∂pα
(42)
(
∂qα
(43)
∂p0,k α
∂q
⎞ ⎟= ⎟ ⎠
* ∂p0, k
(44)
α
∂q
(45)
α
( pα* ⊗ pα ) = ∂qα ⊗ pα + pα* ⊗ ∂qα = 0 , (46) ∂pα
and, from the previous expression:
∂pα*
α
∂q
⊗ pα = − pα* ⊗
∂pα
1 α = − pα* ⊗ ξα eα( ) ⊗ pα . (47) 2 ∂q α
Multiplication of the previous expression with pα* on the right side gives:
22 ▪ VOL. 38, No 1, 2010
∂qα
⊗ ... ⊗ q1* =
(52)
1 = ξα eα ⊗ p0,k , 2
(53)
1 * = − ξα p0, k ⊗ eα . 2
(54)
Substituting (53) and (54) in (49), one can get following expression: ∂ρ kk
∂qα
∂pα*
and
then ∂pα*
= pk* ⊗ ... ⊗
then (50) and (51) become:
⎞ ⎟= ⎟ ⎠
Also, taking into account:
∂
⊗ ... ⊗ pk =
α * eα( ) = p0, α −1 ⊗ eα ⊗ p0,α −1 ,
⎛ α ξ qα ξ qα 1 α = − ξα eα( ) ⊗ ⎜ −eα( ) sin α − cos α ⎜ 2 2 2 ⎝
p* ⊗ p = 1 ,
∂qα
Since
⎞ ⎟= ⎟ ⎠
⎛ α ξ qα ξ qα 1 α = ξα eα( ) ⊗ ⎜ eα( ) sin α + cos α ⎜ 2 2 2 ⎝ 1 α = ξα eα( ) ⊗ pα . 2
∂pα
1 α = − ξα pk* ⊗ ... ⊗ pα* ⊗ eα( ) ⊗ pα* −1 ⊗ ... ⊗ p1* = 2 1 α * = − ξα pα* −1,k ⊗ eα( ) ⊗ p0, (51) α −1 . 2
)
1 α α = ξα −eα( ) ⊗ eα( ) ⊗ 2
⎛ ξ qα ξ qα α ⊗ ⎜ − sin α + eα( ) cos α ⎜ 2 2 ⎝
= p1 ⊗ p2 ⊗ ... ⊗
* ∂p0, k
then, multiplication of (42) with (43) on the left side, (43) becomes: ∂q
*
and
α α −eα( ) ⊗ eα( ) = 1
α
∂q
)=
1 α = ξα p1 ⊗ ... ⊗ pα −1 ⊗ eα( ) ⊗ pα ⊗ ... ⊗ pk = 2 1 α = ξα p0,α −1 ⊗ eα( ) ⊗ pα −1,k , (50) 2
Since
∂pα
α
(k ) ( k ) ∂p0,k , (49) * ⊗ ρkk ⊗ p0, k + p0, k ⊗ ρ kk ⊗ α ∂q ∂qα
α
⎞ ⎟. ⎟ ⎠
=
∂p0,k
∂p0,k
⎛ ξ qα ξ qα α + eα( ) cos α ⎜ − sin α ⎜ 2 2 ⎝
(
(k ) * ∂ p0,k ⊗ ρ kk ⊗ p0, k
where
and
1 = ξα 2
(48)
In the case α ≤ k, generalized coordinate partial derivative of vector ρkk is:
where ∂ri
∂q
1 α = − ξα pα* ⊗ eα( ) . 2
∂q
1 (k ) * = ξα eα ⊗ p0,k ⊗ ρ kk ⊗ p0, k − 2
1 (k ) * − ξα p0,k ⊗ ρ kk ⊗ p0, k ⊗ eα = 2 1 1 = ξα eα ⊗ ρ kk − ξα ρ kk ⊗ eα = 2 2 1 = ξα ( −eα ⋅ ρ kk + eα × ρ kk + ρ kk ⋅ eα − ρ kk × eα ) = 2 1 = ξα ( −eα ⋅ ρ kk + eα × ρ kk + eα ⋅ ρ kk + eα × ρ kk ) = 2 = ξα eα × ρ kk . (55) FME Transactions
In the case α > k, there follows: ∂ρ kk ∂qα
=0.
(56)
Similar to the previous expressions, it can be written the following: ∂ek
= ξα eα × ek ,
∂qα
aCi = ai =
dTα ( i )
α =1
dt
α =1
qα .
(65)
The second part of the previous expression can be written as: dTα ( i ) dt
(57)
=
∂Tα ( i )
i
∑
β =1 ∂q
β
q β .
(66)
According to (40), it can be written the following:
when α ≤ k, ∂ek
α
∂q
∂Tα ( i )
= 0,
=
∂ 2ri
⇒
∂Tα ( i )
=
∂Tβ ( i )
,
(67)
∑ Tα (i ) qα + ∑ ∑ Γαβ (i ) qα q β ,
(68)
∂q β
(58)
∂qα ∂q β
∂q β
∂qα
then (65) becomes:
when α > k, ∂ρi
= ξα eα × ρi .
∂qα
∂qα
= 0,
∂q
=
i
α =1
i
∑ (ξα eα × ρkk + ξk ξα eα × ek q k ) +
Γαβ ( i ) =
+ξα eα + ξα eα × ρi =
(69)
Γαβ ( i ) = Γ βα ( i ) .
(70)
(
)
(61)
when ∀α ≤ i , and: Tα ( i ) = 0
(62)
when ∀α > i . According to (61) and (62), the expression for velocity of the centre inertia of the rigid body (Vi) is the following: n
∑ Tα (i ) qα
.
(63)
α =1
If vectors in (61) are expressed in local body-fixed coordinate frames, then quasibasic vectors become:
In the case α ≤ β then (see (55)):
α * Tα ( i ) = ξα p0,α −1 ⊗ eα( ) ⊗ p0, α −1 ×
(
)
⎡ i (k) (k) * * k⎞ × ⎢ ∑ ⎜⎛ p0,k ⊗ ρkk ⊗ p0, k + ξk p0,k −1 ⊗ ek ⊗ p0,k −1 q ⎟ + ⎝ ⎠ ⎢⎣k =α (i ) * ⎤ + p0,i ⊗ ρi ⊗ p0, i⎥+ ⎦ α) ( * (64) +ξα p0,α −1 ⊗ eα ⊗ p0, α −1
(
)
when ∀α ≤ i . 3.3 Acceleration of the inertia centre of the rigid body (Vi)
Acceleration of the inertia centre of the rigid body (Vi) is time derivative of (39): FME Transactions
∂Tβ ( i ) ∂qα
= ξα eα × Tβ ( i ) ,
(71)
= ξ β e β × Tα ( i ) .
(72)
and, in the case α > β: Γαβ ( i ) =
∂Tα ( i ) ∂q β
The expressions (71) and (72) can be written in the unique form: Γαβ ( i ) = ξinf (α , β )einf (α , β ) × Tsup(α , β )( i ) .
(73)
The expression (65) can be written in the following way: ai =
n
n
n
∑ Tα (i ) qα + ∑ ∑ Γαβ (i ) qα q β
α =1
.
(74)
α =1 β =1
The vectors in (73), expressed in local body-fixed coordinate frames have the following form:
)
(
∂q β
and
Γαβ ( i ) =
⎡ i ⎤ = ξα eα × ⎢ ∑ ρ kk + ξ k e k q k + ρi ⎥ + ξα eα ⎢⎣ k =α ⎥⎦
∂Tα ( i )
,
k =α
Vi =
i
α =1 β =1
(60)
when α > i. According to the previous expressions, (40) becomes: α
i
where
∂ρi
∂ri
ai =
(59)
Also, when α ≤ i, and
Tα ( i ) =
i
i
∑ Tα (i ) qα + ∑
einf (α , β ) =
( inf (α , β ) ) ⎛ ⎞ * = ⎜ p0,inf (α , β )−1 ⊗ einf α , β ⊗ p0,inf α , β ) −1 ⎟ , (75) ( ( ) ⎝ ⎠
)
(
α * Tsup(α , β )( i ) = ξsup(α , β ) p0,α −1 ⊗ eα( ) ⊗ p0, α −1 ×
(
)
⎡ i (k) (k) * * k⎞ × ⎢ ∑ ⎛⎜ p0,k ⊗ ρkk ⊗ p0, k + ξk p0,k −1 ⊗ ek ⊗ p0,k −1 q ⎟ + ⎝ ⎠ ⎢⎣k =α (i ) * ⎤ + p0,i ⊗ ρi ⊗ p0, i⎥+ ⎦
(
)
α * +ξα p0,α −1 ⊗ eα( ) ⊗ p0, α −1 .
(76)
VOL. 38, No 1, 2010 ▪ 23
3.4 Angular velocity of the rigid body (Vi)
Ek ( i ) =
Angular velocity of the rigid body (Vi) can be obtained from the following expression: ωi =
+
i
∑ ξα eα qα ,
(77)
α =1
or
n
Ek = ∑ Ek ( i ) =
α
If unit vector of axis in (77) is expressed in the local body-fixed reference frames, then (77) has the following form: ωi =
∑ ξα ( i
α =1
)
α * α p0,α −1 ⊗ eα( ) ⊗ p0, α −1 q .
+
i =1 n
1 ∑ 2 i =1
Consider an open chain of the rigid bodies system (V1), (V2), ..., (Vn). Differential of kinetic energy of the body (Vi) (Fig. 6) is: (80)
where Ci represents inertia centre, dVi is infinitesimal volume of the body (Vi) which corresponds to infinitesimal mass dmi.
(84)
1 n ∑ mi VCi ⋅ VCi + 2 i =1
∫ ( ωi × ρi ) ⋅ ( ωi × ρi ) dmi .
(85)
(Vi )
Since
(79)
4. KINETIC ENERGY OF THE RIGID BODIES SYSTEM
1 2 , dEk ( i ) = dmi VM i 2
∫ ( ωi × ρi ) ⋅ ( ωi × ρi ) dmi .
(Vi )
Kinetic energy of the system of the rigid bodies is equal to the sum of kinetic energies of each body:
⎧ξ e , ∀α ≤ i ωi = ∑ Ωα ( i ) q , Ωα ( i ) = ⎨ α α . (78) ∀α > i ⎩0, α =1 n
1 2
1 mi VCi ⋅ VCi + 2
VCi =
∂rC
n
∑ ∂qαi qα
(86)
α =1
then VCi ⋅ VCi =
n
∂rC
n
∂rC
∑ ∑ ∂qαi ⋅ ∂q βi qα q β .
(87)
α =1 β =1
The second part of (85), using (78), can be transformed as follows: ω i × ρi =
∑( n
α =1
)
Ωα ( i ) × ρi qα =
n
∂ρ
∑ ∂qαi qα
(88)
α =1
then, it becomes: n
∑ ∫ ( ωi × ρi ) ⋅ ( ωi × ρi ) dmi = i =1 (Vi )
n
n
=∑∑
n
∂ρi
∑ ∫
⋅
α
i =1 α =1 β =1 (Vi ) ∂q
∂ρi α β q q dmi , ∂q β
(89)
Substituting (87) and (89) in (85), it can be obtained: Ek =
∂rCi ∂rCi 1 n n ⎡n ⎢ ∑ mi α ⋅ β + ∑ ∑ 2 α =1 β =1 ⎢⎣ i =1 ∂q ∂q
⎤ α β d m ∫ ∂qα ∂q β i ⎥⎥ q q = i =1 (Vi ) ⎦ n
∂ρi
+∑ Figure 6. Characteristic vectors of the rigid body (Vi)
=
Velocity of the point Mi which belongs to the body (Vi) is: VM i = Vi = VCi + ωi × ρi .
(81)
1 2
(Vi )
ρi dmi = mi ρCi = 0 ,
the expression (82) becomes: 24 ▪ VOL. 38, No 1, 2010
(83)
(90)
where
i =1
n
+∑
∫
∂rCi ∂rCi ⋅ + ∂qα ∂q β
∂ρi
α
i =1 (Vi ) ∂q
Due to (see[3]):
∫
1 n n ∑ ∑ aαβ qα q β , 2 α =1 β =1 n
∫ ( VCi + ωi × ρi ) ⋅ ( VCi + ωi × ρi ) dmi . (82)
(Vi )
∂ρi
aαβ = ∑ mi
Kinetic energy of the body (Vi) is: Ek ( i ) =
⋅
⋅
∂ρi
∂q β
dmi .
(91)
Coefficients ααβ are called the covariant coordinates
[ ]
n× n of the basic metric tensor, and matrix aαβ ∈ R are
called basic metric tensor. According to (40), the first part of (91) becomes: FME Transactions
∂rC
n
∂rC
n
tr rot and aαβ denote translational and rotational where aαβ component of covariant coordinates:
∑ mi ∂qαi ⋅ ∂q βi = ∑ mi Tα (i ) ⋅ Tβ (i ) = i =1
i =1
(
n
= ∑ mi Tα ( i ) i =1
){Tβ ( ) } .
(92)
i
The second part of the right side of (91) can be transformed in the following way: n
∑∫
∂ρi
⋅
∂ρi
α ∂q β i =1 (Vi ) ∂q
n
=∑
∫
i =1 (Vi )
dmi =
(93)
2
n
n
∑∫
∂ρi
α
i =1 (Vi ) ∂q
⋅
∂ρi
(94)
∂q β
i =sup(α , β )
{ }
{ }
= ∑ ξα ξ β ( eα ) ⎡⎣ J Ci ⎤⎦ e β , i =1
(95)
where ⎡ JC ⎤ = ⎣ i⎦
∫
(Vi )
2 ⎡ ρid ⎤ dmi ⎣ ⎦
=
⎡ηi2 + ζ i2 −ξiηi −ξiζ i ⎤ ⎢ ⎥ ζ i2 + ξi2 −ηiζ i ⎥ dmi = ∫ ⎢ −ηiξi ⎥ (Vi ) ⎢⎢ −ζ ξ −ζ iηi ηi2 + ξi2 ⎥ i i ⎣ ⎦
(96)
denotes inertia tensor of the rigid body (Vi). It is most convenient for the inertia tensor of the body (Vi) to be expressed in local body-fixed reference frame 0ξiηiζi, because, in this case, the inertia tensor is constant. After that, covariant coordinates have the following form:
(
){Tβ ( ) } +
( )
{ }
n
aαβ = ∑ mi Tα ( i ) i =1
n
i
i (i ) + ∑ ξα ξ β eα( ) ⎡⎣ J Ci ⎤⎦ e β , i =1
(97)
n
∑
i =sup(α , β )
( )
(
mi Tα ( i )
){Tβ ( ) } +
{ }
i
i tr rot ξα ξ β eα( i ) ⎡⎣ J Ci ⎤⎦ e(β ) = aαβ , (98) + aαβ
FME Transactions
{ }
)
(
)
(101)
Unit vectors in (100), expressed in body-fixed coordinate frame have the following form: i α eα( ) = pα* −1,i ⊗ eα( ) ⊗ pα −1,i = α = p* ⊗ e( ) ⊗ p ,
(102)
(i ) (β ) e β = p*β −1,i ⊗ e β ⊗ pβ −1,i = (β ) = p*β ,i ⊗ e β ⊗ pβ ,i .
(103)
α
α ,i
5. DIFFERENTIAL EQUATIONS OF MOTIONS OF THE RIGID BODY SYSTEM
In this section differential equations of motion of rigid bodies system in covariant form using quaternion algebra will be derived. Consider open chain system of rigid bodies (V1), (V2), ..., (Vn). It is assumed that constraints are holonomic, scleronomic and ideal. A system of independent coordinates (q1, q2,…,qn) can be chosen, which allows that kinetic energy to be written as the function of these coordinates and their time derivatives. In this case, differential equations of motion can be represented in the form of Lagrange’s equation expressed only as the function of generalized coordinates (q1, q2,…,qn) and their time derivatives: d ⎛ ∂Ek ⎜ dt ⎜⎝ ∂q γ
⎞ ∂Ek ⎟⎟ − γ = Qγ , ⎠ ∂q
(104)
where Qγ denotes generalized force of active forces system which act on the rigid body system, which corresponds to generalized coordinate qγ. Generalized velocity partial derivatives of kinetic energy are: ∂Ek
or
i =sup(α , β )
( )
and
⎛ ⎞ 2 = ∑ ξα ξ β ( eα ) ⎜ ∫ ⎡ ρid ⎤ dmi ⎟ e β = ⎣ ⎦ ⎜⎜ ⎟⎟ i =1 ⎝ (Vi ) ⎠ n
(99)
i
i ξα ξ β eα( i ) ⎡⎣ J Ci ⎤⎦ e(β ) . (100)
α ,i
dmi =
n
n
∑
(
then (93) becomes:
∑
){Tβ ( ) } ,
aαβ q1 ,..., q n = aβα q1 ,..., q n .
( eα × ρi ) {eβ × ρi } = − ( eα ) ⎡⎣ ρid ⎤⎦ {eα } ,
+
mi Tα ( i )
From (98) it can be concluded that coefficients aαβ have the following property:
Since (see[3]):
aαβ =
i =sup(α , β )
and rot aαβ =
ξα ξ β ( eα × ρi ) ⋅ ( e β × ρi )dmi .
(
n
∑
tr aαβ =
q γ
=
1 n 1 n aγβ q β + ∑ aαγ qα . ∑ 2 β =1 2 α =1
(105)
According to (98), (105) can be written as: ∂Ek q γ
=
n
∑ aαγ qα .
(106)
α =1
VOL. 38, No 1, 2010 ▪ 25
einf (α , β ) =
Time derivative of (106) is: ⎞ n α ⎟⎟ = ∑ aαγ q + ⎠ α =1 n n ⎛ ∂a ∂aβγ ⎞ α β 1 αγ + ∑ ∑⎜ + ⎟ q q . β ⎜ 2 α =1 β =1 ⎝ ∂q ∂qα ⎟⎠
( inf (α ,β ) ) * = p0,inf (α , β )−1 ⊗ einf α , β ⊗ p0,inf (α , β )−1 . (116) ( )
d ⎛ ∂Ek ⎜ dt ⎜⎝ q γ
(107)
The rotational components of the Christoffel symbols are as follows: ⎛ ∂e( i ) β = ∑ ξ β ξγ ⎜⎜ ∂qα ∂qα ⎜ i =sup( β ,γ ) ⎝
⎞ ⎟ ⎡ J ⎤ e( i ) + ⎟ ⎣ Ci ⎦ γ ⎟ ⎠ ⎧ ∂e( i ) ⎫ n i) ⎪ γ ⎪ ( ⎡ ⎤ . (117) + ∑ ξ β ξγ e β ⎣ J Ci ⎦ ⎨ α ⎬ ⎪⎩ ∂q ⎪⎭ i =sup( β ,γ )
rot ∂aβγ
Generalized coordinate partial derivatives of kinetic energy are: ∂Ek ∂qγ
=
1 n n ∂aαβ α β q q . ∑∑ 2 α =1 β =1 ∂qγ
n
∑ aαγ qα +
α =1
n
n
∑ ∑ Γαβ ,γ qα q β
α =1 β =1
( )
(108)
Substituting (107) and (108) in (104), the Lagrange equations have the following form [3]: = Qγ ,
(109)
According to (102) and (103), and doing the same as in Section 3.2, partial derivatives of unit vectors of the axis in the case α ≥ β,γ, are:
(i ) ∂e β α
where
∂q
Γαβ ,γ =
1 ⎛ ∂aβγ ∂aγα ∂aαβ + − ⎜ 2 ⎜⎝ ∂qα ∂q β ∂qγ
⎞ ⎟⎟ ⎠
(110)
denotes Christoffel symbols of the first kind. This form of equations is called covariant. According to (98), it can be written: tr rot Γαβ ,γ = Γαβ ,γ + Γαβ ,γ ,
(111)
i ∂eγ( )
α
∂q
tr tr tr 1 ⎛⎜ ∂aβγ ∂aγα ∂aαβ + − 2 ⎜ ∂qα ∂q β ∂qγ ⎝
⎞ ⎟ ⎟ ⎠
=
∂q
(112)
denotes the translational components, and rot rot rot 1 ⎛⎜ ∂aβγ ∂aγα ∂aαβ rot Γαβ = + − ,γ 2 ⎜ ∂qα ∂q β ∂qγ ⎝
⎞ ⎟ ⎟ ⎠
(113)
denotes rotational components of Christoffel symbols. Deriving (99), applying the following properties: ∂Tα ( i ) ∂q β
∂ri
=
∂qα ∂q β
=
∂Tβ ( i ) ∂qα
(114)
=
n
∑
i =sup(α , β ,γ )
n
∑
i =sup(α , β ,γ )
mi
∂Tα ( i ) ∂q β
where
26 ▪ VOL. 38, No 1, 2010
i
(119)
i =sup( β ,γ )
n
i =sup( β ,γ )
(
)
{ }
i (i ) ⎡ J ⎤ e( i ) + ξα ξ β ξγ e(β ) × esup (α , β ) ⎣ Ci ⎦ γ
( )
{
}
i (i ) ξα ξ β ξγ e(β ) ⎡⎣ J Ci ⎤⎦ eγ( i ) × esup . (120) (α ,γ )
Doing similarly with other components as in (113), and substituting it into (126), the rotational components of the Christoffel symbols become:
( ) { } () () () ⎡ J ⎤ e( ) + + ( e ) ⎣⎡ J ⎦⎤ {e( ) × e + e( ) × e ( )} ( ( ) ) ⎣ ⎦{ } () () ⎡ J ⎤ e( ) − + ( e( ) ) ⎣⎡ J ⎦⎤ {e( ) × e − e( ) × e ( )} ( ( ) ) ⎣ ⎦{ } () () ⎞. − ( e( ) ) ⎡⎣ J ⎤⎦ {e × e (121) ( ) } ⎟⎠
rot Γαβ ,γ =
β
γ
n 1 i (i) ⎡ J ⎤ e( i ) + ξα ξβ ξγ ⎜⎛ e(β ) × esup ∑ (α ,β ) ⎣ Ci ⎦ γ 2 i=sup(α ,β ,γ ) ⎝ i
i sup α ,γ
γ
i
i sup α ,β
α
Ci
γ
Ci
α
i
α
Ci
i
i sup β ,γ
Ci
α
i
i sup α ,γ
Ci
β
i
i
i
i sup β ,γ
β
rot Γαβ ,γ =
(
){Tγ ( ) } ,
i i = ξα eγ( ) × eα( ) .
According to the certain properties of members in the previous expression, (121) becomes:
⋅ Tγ ( i ) =
miξinf (α , β ) einf (α , β ) ×
×Tsup(α , β )( i )
n
∑
∑
i
(71), (72) and (73), and substituting it in (112), the translational components of the Christoffel symbols become: tr Γαβ ,γ =
+
i
,
(118)
Substituting (118) and (119) into (117), it can be obtained: α
tr Γαβ ,γ =
i (i ) = ξα e β × eα( ) ,
and
rot ∂aβγ
where
{ }
n
( ) { } () () − ( e( ) × e( ) ) ⎣⎡ J ⎦⎤ {e } − ( e × e( ) ) ⎣⎡ J ⎦⎤ {e( ) } ⎟⎞ . (122) ⎠
=
(115)
n 1 (i ) (i) ⎡ J ⎤ e( i ) − ×e ∑ ξαξβ ξγ ⎛⎜⎝ einf (α ,β ) sup(α ,β ) ⎣ Ci ⎦ γ 2 i =sup(α ,β ,γ )
i
α
i
γ
Ci
i
β
i
β
i
γ
Ci
i
α
FME Transactions
Due to the property of dual object, the inertia tensor can be written in the following way [3]: ⎡ JC ⎤ = − ⎣ i⎦
2
∫
(Vi )
⎡ ρid ⎤ dmi = ⎣ ⎦
∫ {ρi [ I ] − {ρi }( ρi )}dmi , (123) 2
(Vi )
and substituting (123) in (122), the Christoffel symbols become: rot Γαβ ,γ =
=
(
n 1 ∑ ξα ξ β ξγ 2 i =sup(α , β ,γ )
)
i
i
γ
β
i
{ }
i
γ
i
i
α
i
i
i
β
i
( () + (e (
(124)
i
i
γ
β
i
i
α
i
) { } () × e( ) ) {ρ }( ρ ) {e . ) ( )}
i (i ) (i ) = einf α , β × eγ( ) {ρi }( ρi ) esup α , β + ( ) ( ) i sup α , β
i
γ
i
i inf α , β
i
(125)
Subintegral function can be easily transformed in the following expression:
(
)
{ }
i (i ) (i ) − einf α , β × esup α , β ⎡ ρid ⎤ ⎡ ρid ⎤ eγ( ) + ( ) ( ) ⎣ ⎦⎣ ⎦
( () = (e (
){ρ }(ρ ){e( ) ( ) } = () × e( ) ) {ρ }( ρ ) {e , ) ( )}
(i ) i + einf α , β × eγ( ) ( ) i sup α , β
i
γ
i
i sup α , β
i
i
i inf α , β
i
(126)
and, the rotational components of the Christoffel symbols become: rot Γαβ ,γ =
=
∫
(Vi =
{) ( ( ) (
n
ξα ξ β ξγ
)
{
}}
i i (i ) esup α , β × eγ( ) {ρi }( ρi ) einf α , β dmi = ) ( )
n
∑
i =sup(α ,β ,γ )
(
) {
}
i (i) (i) , (127) ξα ξβ ξγ esup × e( ) Π e (α ,β ) γ [ i ] inf (α ,β )
where
[Π i ] = ∫ {ρi }( ρi ) dmi
(128)
(Vi )
denotes the planar moment inertia [3]. Unit vectors in (127) can be expressed in local-body fixed coordinate FME Transactions
0,inf (α , β ) −1 ⊗
{ }
⎞ ( inf (α ,β ) ) ⎞ * ⊗einf α , β ⊗ p0,inf (α ,β )−1 ⎟⎠ × Tsup(α ,β )( i ) ⎟ Tγ ( i ) + ( ) ⎠ n
∑
i =sup(α , β ,γ )
⎛ * ( sup(α ,β ) ) ξα ξ β ξγ ⎜ ⎜⎛ psup α , β ),i ⊗ esup(α , β ) ⊗ ( ⎝⎝
)(
) ) [Π ] i
(129)
This paper has shown the development of Lagrange’s equations of the second kind of the rigid bodies system in the covariant form using the quaternion algebra. It can be concluded that every vector which belongs to the arbitrary body of the rigid bodies system can be easily expressed in the body-fixed reference frame of another body making composite quaternion, which consists of Hamiltonian product of quaternions representing the rotation neighbouring bodies, avoiding trigonometric functions characteristic of Euler’s angles. Also, it is easy to find generalized coordinate partial derivatives of that vector. Unlike the existing results, where quaternionic approach has been applied only for the case of rotation of one or two bodies, it is here presented the procedure of obtaining the model of multi-body system of n rigid bodies in terms of quaternions, which is useful for studying kinematics, dynamics as well as for research of control system designs. ACKNOWLEDGMENT
This work is partially funded by the Ministry of Science and Environmental Protection of Republic of Serbia as T.R.P. No. 20152. REFERENCES
∑
i =sup(α , β ,γ )
(( p
6. CONCLUSION
(e( ) × e( ) ){ρ }(ρ ){e( ) } + (e( ) × e( ) ){ρ }(ρ ){e( ) } = i
miξinf (α , β )
i
The last two terms of subintegral function in previous expression can be written as follows: α
i =sup(α , β ,γ )
( inf (α , β ) ) ⎧ * ⎫ ⎨ pinf (α , β ),i ⊗ einf (α , β ) ⊗ pinf (α , β ),i ⎬ . ⎩ ⎭
β
i
i
n
∑
γ ⊗ psup(α , β ),i × pγ* ,i ⊗ eγ( ) ⊗ pγ ,i
){ρ }(ρ ){e( ) } + () + ( e × e( ) ) {ρ }( ρ ) {e( ) } ⎞⎟ dm . ⎠ (
i i + eα( ) × eγ( )
Γαβ ,γ =
+
2 (i ) ⎛ (i ) (i ) ⎡ d⎤ ⎜ − einf (α , β ) × esup(α , β ) ⎣ ρi ⎦ eγ + (Vi ) ⎝
∫
frame like in (102), so the Christoffel symbols finally become:
[1] Hamilton, W.R.: On Quaternions, or a new system of imaginaries in algebra, The London, Edinburgh and Dublin Philosophical Magazine and Journal of Science, Vol. 25-36, 1844-1850. [2] Markovic, S.: Rapid Establishment of Differential Equations of Motion of Rigid Bodies System in an Analytical Form, MSc thesis, Faculty of Mechanical Engineering, University of Belgrade, Belgrade, 1992, (in Serbian). [3] Covic, V. and Lazarevic M.: Mechanics of Robots, Faculty of Mechanical Engineering, University of Belgrade, Belgrade, 2009, (in Serbian). [4] Isenberg, D.R. and Kakad, Y.P.: Computed torque control of a quaternion based space robot, in: Proceedings of the 19th International Conference VOL. 38, No 1, 2010 ▪ 27
on System Engineering, 19-21.08.2008, Las Vegas, USA, pp. 59-64. [5] Arribas, M., Elipe, A. and Palacios, M.: Quaternions and the rotation of a rigid body, Celestial Mechanics and Dynamical Astronomy, Vol. 96, No. 3-4, pp. 239-251, 2006. [6] Kuipers, J.B.: Quaternions and Rotation Sequences: A Primer with Applications to Orbits, Aerospace and Virtual Reality, Princeton University Press, New Jersey, 1999.
КИНЕМАТИКА И ДИНАМИКА СИСТЕМА КРУТИХ ТЕЛА У КВАТЕРНИОНСКОЈ ФОРМИ: ЛАГРАНЖЕВА ФОРМУЛАЦИЈА У
28 ▪ VOL. 38, No 1, 2010
КОВАРИЈАНТНОМ ОБЛИКУ – РОДРИГОВ ПРИСТУП Немања Д. Зорић, Михаило П. Лазаревић, Александар М. Симоновић
У овом раду се предлаже кватернионски приступ за моделирање кинематике и динамике система крутих тела. Уместо регуларног „Њутн-Ојлеровог“ и Лагранжевог метода коришћеног на традиционалан начин, употребљавају се Лагранжеве једначине друге врсте у коваријантном облику применом Родриговог приступа и кватернионске алгебре. Добијен је модел система од n крутих тела у кватернионској форми који је користан за проучавање кинематике, динамике система за општи случај кретања, као и за синтезу система управљања.
FME Transactions
