SLIDE 1 52 Basilio Bona - Dynamic Modelling The vector x is simply obtained taking the first three elements of the last column
x ≡ p1 p2 p3 ⇐ t1 t2 t3 ≡ t (2.68) Figure 2.16: Dato T calcolare p. Notice that in this way the obtained coordinates are relative to the origin of the reference frame described by T; if we need to compute the coordinates of a different geometrical point, we should characterize this new point as a vector in RB and transform it using eqn. (2.55). To compute α from R it is necessary to define the physical meaning of the three components αi of α. We assume to use the Euler angles or the RPY angles. It exists the inverse nonlinear function αE = g −1
E (R) that gives the Euler angles
from the elements rij of a rotation matrix R (2.73), and another inverse nonlinear function αRPY = g −1
RPY (R) that gives the RPY angles from the elements rij of a
rotation matrix R (2.77). The inverse solution, if exists, could be non unique, since a finite number of angles α may exist that produce the same rotation matrix R. Since inverse trigonometric functions are involved, we consider as equaivlent solutions those that differ for integer multiples of 2π. In the following Section we will illustrate the most used ways to characterize the
- rientation of a rigid body in space.
2.12 Orientation Parameters
We have seen that a rotation is a geometrical transformation acting on R3, defined by a orthonormal matrix R. At the same time the orientation of a rigid body, with a local reference frame attached to it, is represented by the rotation of the local frame with respect to the world or global frame. We conclude that it is equivalent to speak of the orientation of a rigid body or of its reference frame with respect to
SLIDE 2
Basilio Bona - Dynamic Modelling 53 some other “fixed” frame Body B Orientation ⇔ Rotation Matrix RB We recall that a rotation is characterized by only three parameters, and all the repre- sentation described in this section will be a different way to “organize” such param- eters; in eqn. (2.65) these three parameters were indicated as α = [ α1 α2 α3 ]T, but there are other parameterizations that use more than three parameters, as the quaternions, introduced in Section 2.12.6. Another example comes from the Euler theorem, that states that any composition of rotations is always a rotation of an angle θ around an axis defined by a unit vector u; therefore we can represent a ro- tation by the set (u, θ), where u has two free parameters, since there is an implicit constraint ∥u∥ = 1), and the third one coincide with θ. Other authors use a non unit vector v, whose norm provides the angle value ∥v∥ = θ. Now we describe the most common forms used to represent the orientation of a rigid body in R3.
2.12.1 Direction Cosines
Direction cosines are nothing else that the rotation matrix R itself. Indeed R contains in its columns (or rows) the representation of the unit basis vectors of the local frame with respect to the world frame. This representation needs nine parameters that must obey to six unit norm orthogonality constraints. In this case the parameters [ α1 α2 α3 ]T are hidden in R but can be extracted using the equations (2.12.2) e (2.12.3).
2.12.2 Euler Angles
Historically this is the first representation of the orientation of a body: it associates to α1, α2, α3 three angles, called Euler angles and usually denoted by φ, θ, ψ. To understand how to build the Euler angles it is necessary to define them through an implicit procedure. The orientation of a mobile reference frame Rm described by the Euler angles is obtained by three successive rotations around the principal axes, following a precise rule First rotation, angle φ Rz,φ ≡ R(k, φ) = cφ −sφ sφ cφ 1 rotation of an angle φ around the local (mobile) axis z.
SLIDE 3 54 Basilio Bona - Dynamic Modelling Second rotation, angle θ Rx,θ ≡ R(i, θ) = 1 cθ −sθ sθ cθ rotation of an angle θ around the local (mobile) axis x. Third rotation, angle ψ Rz,ψ ≡ R(k, ψ) = cψ −sψ sψ cψ 1 rotation of an angle ψ around the local (mobile) axis z. Using the rule “pre–fixed” “post–mobile” we compute the complete rotation matrix based on the Euler angles as R (φ, θ, ψ) ≡ Rz,φRx,θRz,ψ ≡ R(k, φ)R(i, θ)R(k, ψ) = cφcψ − sφcθsψ −cφsψ − sφcθcψ sφsθ sφcψ + cφcθsψ −sφsψ + cφcθcψ −cφsθ sθsψ sθcψ cθ (2.69) This composition rule is not unique in technical literature, since many textbooks adopt a slightly different convention: the second rotation around the x axis is re- placed by a rotation around the mobile axis y, producing a different “Euler” matrix:
- R (φ, θ, ψ) ≡ Rz,φRy,θRz,ψ ≡ R(k, φ)R(j , θ)R(k, ψ) =
−sφsψ + cφcθcψ −sφcψ − cφcθsψ cφsθ cφsψ + sφcθcψ cφcψ − sφcθsψ sφsθ −sθcψ sθsψ cθ (2.70) In this notes we will always adopt the first form (2.69), but we recall that in many aerospace engineering textbooks the form (2.70) is widely used. The above formulas are also called direct relations, since, given the three Euler angles they compute the rotation matrix R(φ, θ, ψ). The inverse relation, i.e. how to compute the Euler angles given an generic R matrix is solved considering the generic elements of a matrix, provided it is orthonormal R = r11 r12 r13 r21 r22 r23 r31 r32 r33 , (2.71) where the elements rij are known. To obtain the Euler angles it is necessary to solve
SLIDE 4 Basilio Bona - Dynamic Modelling 55 the following nonlinear equation system: r11 = cφcψ − sφcθsψ r12 = −cφsψ − sφcθcψ r13 = sφsθ r21 = sφcψ + cφcθsψ r22 = −sφsψ + cφcθcψ r23 = −cφsθ r31 = sθsψ r32 = sθcψ r33 = cθ (2.72) that has the following generic solution θ = ± arccos (r33) ± 2kπ ψ = ± arccos (r32 sθ ) ± 2kπ φ = ± arccos (−r23 sθ ) ± 2kπ (2.73) Unfortunately this solution presents some drawbacks
- 1. the inverse trigonometric function arccos(·) is not unique: indeed cos(θ) =
cos(−θ);
- 2. the solution becomes not definite for r33 = 1, i.e., when sθ = 0; in this case
the angles φ and ψ are not uniquely known; only their sum is given;
- 3. when θ → 0◦ or θ → ±180◦, the second and the third equations provide inac-
curate solutions, since the numeric accuracy of the arccos(·) function depends
To solve these drawbacks, instead of arccos(·) it is customary to use the function atan2(y, x), that is available in all the mathematical libraries of the most used computer languages. It is definite as: θ = atan2(y, x) = tan−1 (y x ) = = 0◦ ≤ θ ≤ 90◦ if x ≥ 0; y ≥ 0 90◦ ≤ θ ≤ 180◦ if x ≤ 0; y ≥ 0 −180◦ ≤ θ ≤ −90◦ if x ≤ 0; y ≤ 0 −90◦ ≤ θ ≤ 0◦ if x ≥ 0; y ≤ 0 (2.74) Moreover, by default, atan2(0, 0) = 0.
SLIDE 5
56 Basilio Bona - Dynamic Modelling Using this function, the solution to (2.72) is: φ = atan2 (r13, −r23) ± 2kπ ψ = atan2 (−cφr12 − sφr22, cφr11 + sφr21) ± 2kπ θ = atan2 (sφr13 − cφr23, r33) ± 2kπ (2.75) Euler angles singularity We observe that when r33 = 1 the matrix R(φ, θ, ψ) is equal to the elementary matrix R(k, γ), that is function of a single angle. In this case we say that the Euler representation is singular; from the three possible angles we can obtain only two angles; from (2.73) we have θ = 0, and the product R(k, φ)R(i, θ)R(k, ψ) reduces to R(k, φ)R(k, ψ) = R(k, (φ + ψ)) from which we have γ = (φ + ψ); we cannot compute separately the two angles phi and ψ, but only their sum. This situation is described saying that θ does not decouple any more the other two rotations and so a singular configuration is produced.
2.12.3 RPY Angles
Also in the case of Roll-Pitch-Yaw angles (RPY for short) θx, θy, θz it is necessary to define them through an implicit procedure. The orientation of a mobile reference frame Rm described by the RPY angles is obtained by three successive rotations around the principal axes, following a precise rule First rotation, angle θx Rx,θx ≡ R(i, θx) = 1 cθx −sθx sθx cθx rotation of an angle θx around the world (fixed) axis x. Second rotation, angle θy Ry,θy ≡ R(j , θy) = cθy sθy 1 −sθy cθy rotation of an angle θy around the world (fixed) axis y. Third rotation, angle θz Rz,θz ≡ R(k, θz) = cθz −sθz sθz cθz 1
SLIDE 6 Basilio Bona - Dynamic Modelling 57 rotation of an angle θz around the world (fixed) axis z. Using the rule “pre–fixed” “post–mobile” we compute the complete rotation matrix based on the RPY angles as R (θx, θy, θz) ≡ Rz,θzRy,θyRx,θx ≡ R(k, θz)R(j , θy)R(i, θx)
def
= cθzcθy sθxsθycθz − cθxsθz cθxsθycθz + sθxsθz cθysθz sθxsθysθz + cθxcθz cθxsθysθz − sθxcθz −sθy sθxcθy cθxcθy (2.76) The RPY angles are computed applying the same approach adopted for the Euler angles, yielding: θx = atan2 (r32, r33) ± 2kπ θz = atan2 (−cθxr12 + sθxr13, cθxr22 − sθxr23) ± 2kπ θy = atan2 (−r31, sθxr32 + cθxr33) ± 2kπ (2.77) It is interesting to notice that the product R(k, θz)R(j , θy)R(i, θx) may also be read in a different order, starting from left to right, i.e., applying the “post-mobile” rule: first apply a rotation R(k, θz) around axis z, then apply a rotation R(j , θy) around mobile axis y, then apply a rotation R(i, θx) around mobile axis x. The difference is only in the description of the rotations, not in the final result (2.76), that is the same. Other definitions can be found in various textbooks and are sometimes used, for instance, the following alternative sequence is quite common 1) A rotation R(j , θy) of θy around the fixed axis y; 2) A rotation R(k, θz) of θz around the fixed axis z; 3) A rotation R(i, θx) of θx around the fixed axis x. The resulting rotation matrix is: R (θz, θy, θx) ≡ Rx,θxRy,θyRz,θz ≡ R(i, θx)R(j , θy)R(k, θz)
def
= cθycθz −cθysθz sθy sθxsθycθz + cθxsθz −sθxsθysθz + cθxcθz −sθxcθy −cθxsθycθz + sθxsθz cθxsθysθz + sθxcθz cθxcθy (2.78) In this notes we will apply only definition (2.76). RPY angles singularity Roll-Pitch-Yaw angles are subject to singularity too, as the Euler angles, and in general to all three-angles representation.
SLIDE 7 58 Basilio Bona - Dynamic Modelling In particular the following relations hold R(k, θz)R(j , 90◦) = R(j , 90◦)R(i, θz) (2.79) R(i, θx)R(j , 90◦) = R(j , 90◦)R(k, θx) (2.80) When θy = 90◦ the RPY matrix in (2.76) becomes singular, since from (2.79) R(k, θz)R(j , 90◦)R(i, θx) = R(j , 90◦)R(i, θz)R(i, θx) = R(j , 90◦)R(i, (θx + θz)) and also the RPY matrix in (2.78) becomes singular, since from (2.80) R(i, θx)R(j , 90◦)R(k, θz) = R(j , 90◦)R(k, θx)R(k, θz) = R(j , 90◦)R(k, (θx + θz)) Both representation are no more function of three angles, but only of a combination
- f two of them; this fact is the cause of the so called gimbal-lockproblem that
- ccurs in gyroscopes and is well known since the incident on the Apollo 10 Manned
Lunar Spacecraft [22].
2.12.4 Cardan Angles
We have seen in the previous Section that many alternatives are possible in order to define three rotations. All possible angles are called generically Cardan angles. Two possible groups of rotations arise: the first group includes the product obtained by the product of three elementary rotation (all different); we call θ1, θ2 and θ3 the three generic Cardan angles involved; Table 2.12.4 list all possible combinations. The second group includes the product of three rotation matrices, but introducing
- nly two angles; in this case the Cardan angles are called α, β and γ. Table 2.12.4
list all possible combinations. Cardan angles singularities As already noted in Section 2.12.2 and 2.12.3 the sequence of three angles produces a singularity when a certain pattern of angles appear. For the first group it is the presence of a 90◦ angles that may produce a singular behaviour, since the following identities hold R(i, θx)R(j , 90◦) = R(j , 90◦)R(k, θx) R(i, θx)R(k, 90◦) = R(k, 90◦)R(j , θx) R(j , θy)R(i, 90◦) = R(i, 90◦)R(k, θy) R(j , θy)R(k, 90◦) = R(j , 90◦)R(i, θy) R(k, θz)R(i, 90◦) = R(i, 90◦)R(j , θz) R(k, θz)R(j , 90◦) = R(j , 90◦)R(i, θz) (2.81) while for the second group it is sufficient that the middle rotation matrix is equal to the identity to give origin of a singular representation. http://www.astronautix.com/flights/apollo10.htm
SLIDE 8 Basilio Bona - Dynamic Modelling 59 R(i, θ1)R(j , θ2)R(k, θ3)
R(i, θ1)R(k, θ3)R(j , θ2) cθ1cθ3 −sθ3 sθ2cθ3 cθ1cθ2sθ3 + sθ1sθ2 cθ1cθ3 cθ1sθ2sθ3 − sθ1cθ2 sθ1cθ2sθ3 − cθ1sθ2 sθ1cθ3 sθ1sθ2sθ3 + cθ1cθ2 R(j , θ2)R(i, θ1)R(k, θ3) sθ1sθ2sθ3 + cθ2cθ3 sθ1sθ2cθ3 − cθ2sθ3 cθ1sθ2 cθ1sθ3 cθ1sθ3 −sθ1 sθ1cθ2sθ3 − sθ2cθ3 sθ1cθ2cθ3 + sθ2sθ3 cθ1cθ2 R(j , θ2)R(k, θ3)R(i, θ1) cθ2cθ3 −cθ1cθ2sθ3 + sθ1sθ2 sθ1cθ2sθ3 + cθ1sθ2 sθ1 cθ1cθ3 −sθ1cθ3 −sθ2cθ3 cθ1sθ2cθ3 + sθ1cθ2 −sθ1sθ2sθ3 + cθ1cθ2 R(k, θ3)R(i, θ1)R(j , θ2) −sθ1sθ2sθ3 + cθ2cθ3 −cθ1sθ3 sθ1cθ2sθ3 + sθ2cθ3 sθ1sθ2cθ3 + cθ2sθ3 cθ1cθ3 −sθ1cθ2cθ3 + sθ2sθ3 −cθ1sθ2 sθ1 cθ1cθ2 R(k, θ3)R(j , θ2)R(i, θ1)
Table 2.1: Cardan angles obtained by three elementary rotations around three dif- ferent angles.
2.12.5 Euler Parameters
Given the rotation described in (2.27) R(u, θ) of an angle θ around an axis repre- sented by the unit vector u = [ u1 u2 u3 ]T, we introduce four parameters, called Euler parameters vi (not to be confused with the Euler angles), defined as: v1 = u1 sin θ 2, v2 = u2 sin θ 2, v3 = u3 sin θ 2, v4 = cos θ 2 (2.82) Only three out of four of these parameters are independent, since the following constraint holds
4
∑
i=1
v2
i = 1
(2.83) The Euler parameters may be interpreted as the components of a unit quaternion
- u. The quaternions will be introduced in Section 2.12.6.
SLIDE 9 60 Basilio Bona - Dynamic Modelling R(i, α)R(j , β)R(i, γ) cβ sβsγ sβcγ sαsβ −sαcβsγ + cαcγ −sαcβcγ − cαsγ −cαsβ cαcβsγ + sαcγ cαcβcγ − sαsγ R(i, α)R(k, β)R(i, γ) cβ −sβcγ sβsγ cαsβ cαcβcγ − sαsγ −cαsβsγ − sαcγ sαsβ sαcβcγ + cαsγ −sαcβsγ + cαcγ R(j , α)R(i, β)R(j , γ) −sαcβsγ + cαcγ sαsβ sαcβcγ + cαsγ sβsγ cβ −sβcγ −cαcβsγ − sαcγ cαsβ cαcβcγ − sαsγ R(j , α)R(k, β)R(j , γ) cαcβcγ − sαsγ −cαsβ cαcβsγ + sαcγ sβcγ cβ sβsγ −sαcβcγ − cαsγ sαsβ −sαcβsγ + cαcγ R(k, α)R(i, β)R(k, γ)
R(k, α)R(j , β)R(k, γ)
Table 2.2: Cardan angles obtained by two elementary rotations around three differ- ent angles. Given u, θ and the Euler parameters vi, the rotation matrix R(u, θ) can be com- puted as: R(u, θ) = v2
1 − v2 2 − v2 3 + v2 4
2(v1v2 − v3v4) 2(v1v3 + v2v4) 2(v1v2 + v3v4) −v2
1 + v2 2 − v2 3 + v2 4
2(v2v3 − v1v4) 2(v1v3 − v2v4) 2(v2v3 + v1v4) −v2
1 − v2 2 + v2 3 + v2 4
(2.84) Conversely, given R(u, θ), the Euler parameters are computed as: v4 = ±1 2 √ (1 + r11 + r22 + r33) v1 = 1 4v4 (r32 − r23) v2 = 1 4v4 (r13 − r31) v3 = 1 4v4 (r21 − r12) (2.85)
SLIDE 10 Basilio Bona - Dynamic Modelling 69
- 3. in order to compute r given R one builds the skew-symmetric matrix S(r),
given by S(r) = R − RT 1 + tr (R) = −r3 r2 r3 −r1 −r2 r1 = (2.103) and has immediately the elements ri.
Considering again Example 2.12.1, we want to compute the Rodrigues vectors that represent the two rotations R1(i, 90◦) and R2(j , 90◦) and then compute the Ro- drigues vector of the product R = R1(i, 90◦)R2(j , 90◦). The Rodrigues vector relative to R1(i, 90◦) is r 1 = [ 1 ]T, while the Rodrigues vector relative to R2(j , 90◦) is r 2 = [ 1 ]T; The overall rotation corresponds to a Rodrigues vector computed as: R = R1(i, 90◦)R2(j , 90◦) ⇒ r = r 1 ⊙ r 2 = 1 1 1 Since ∥r∥ = √ 3, we have θ 2 = arctan √ 3 = 60◦ ⇒ θ = 120◦.
Orientation Error and Angle-Axis Representation
Often, in satellite or UAV orientation control problems it is necessary to compute the angular error between two different orientations: when Euler or RPY angles are used, the error between the desired αd and the measured ones αm is simply defined by the difference between the respective angles ∆α = αd − αm
∆α′ = αm − αd The first definition ∆α is commonly used by control designers that define as er- ror the difference between the desired and the measured angles, while the second definition ∆α is used by measurement people that call error the opposite difference. Instead, when the rotation matrices are used, it is necessary to define the error in another way since it is a big error to compute the difference between two rotation matrices! Let us assume that Rd is the desired rotation matrix and Rm the measured one; the error may be defined in four different ways:
SLIDE 11 70 Basilio Bona - Dynamic Modelling 1) Re1: defined as RmRe1 = Rd → Re1 = RT
mRd.
2) Re2: defined as RdRe2 = Rm → Re2 = RT
d Rm.
3) Re3: defined as Re3Rm = Rd → Re3 = RdRT
m.
4) Re3: defined as Re4Rd = Rm → Re4 = RmRT
d .
Notice that Re1 = RT
e2 and Re3 = RT e4.
Relations 1) and 2) define the orientation error as a post-product, and therefore are to be understood as a perturbation of the rotation with respect to the “mobile” reference frame, while relations 3) and 4) define the orientation error as a pre- product, and therefore are to be understood as a perturbation of the rotation with respect to the “fixed” or world reference frame. In any case the orientation error is the product of two rotation matrices, and in
- rder to compute the axis unit vector ue and the angle ∆θ it would be necessary to
perform the products and then use the identities (2.43) and (2.46). Fortunately there exist another formula that one can use to compute the angle nd the axis of the rotation resulting from the product of rotation matrices, without actually doing the product. This relation is just used to compute the axis-angle of the orientation error. Assuming to have defined the orientation error as Re = RT
1 R2 – cases 1) and 2) –
then the following identity holds ue sin ∆θ = 1 2 (¯ r 2 × ¯ r 1 + ¯ g 2 × ¯ g 1 + ¯ b2 × ¯ b1 ) (2.104) where ¯ r i, ¯ g i and ¯ bi are the first, second and third rows of Ri, i = 1, 2. One can notice that the left-hand term in (2.104) is a type b) rotation vector, that goes to zero for angles equal to 0 or ±π, for which we know that identity (2.46) in not applicable. If we define the orientation error as Re = R1RT
2 – cases 3) and 4) – then the
following identity, also known as the unit vector lemma, holds u sin θ = 1 2 (r 2 × r 1 + g 2 × g 1 + b2 × b1) (2.105) where r i, g i and bi are the first, second and third columns of Ri, i = 1, 2.
2.12.11 Conclusions
In this Section we have seen that three parameters αi are sufficient to define the
- rientation of a rigid body; the problem with three (angle) parameters is that they
are not singularity-free.
SLIDE 12 Basilio Bona - Dynamic Modelling 71 If we use the quaternions, the Euler parameters, the Cayley-Klein parameters or the Rodrigues vectors, it in unclear what are the parameters that take the place
Using these alternative representations, we cannot say that the αi are “angles”, but it is necessary to speak in more general terms of “angular parameters”, whose knowledge nevertheless allows to compute the chosen (Euler or RPY or Cardan) angles. For instance, using the quaternions we can define: α1 = u1 sin θ 2, α2 = u2 sin θ 2, α3 = u3 sin θ 2 (2.106) and implicitly assume the unit norm. However it is a very common practice to associate to the αi the Euler or the RPY angles, although in the last few years quaternion representation has gained much attention, mainly in the aerospace and computer graphics applications.
2.13 Point Kinematics
Having introduced in the previous Sections the different representations of a geomet- rical point or of a vector, both polar or axial, in a given reference frame, and having also characterized the various types of rigid displacements in the Euclidean space, we are now ready to describe the motion of geometrical points, assumed massless. This description takes the name of kinematics and is distinct from dynamics since the former studies the motion of points or rigid bodies establishing the relations among positions, velocities and accelerations from a pure geometrical point of view, while the latter studies the influence of the forces and torques on the bodies on these quantities. In few words, kinematics study the motion without considering its causes and its effects, while dynamics studies how the external actions on the given bodies are related to their motion. Suppose to have a geometrical point P moving in the 3D Euclidean space with a time law specified by a function P(t); we can assume a vector representation of P in a given reference frame x P(t) = x1P(t) x2P(t) x2P(t)
- r, to keep notation simple
x(t) = x1(t) x2(t) x2(t)
