Attitude Representation

Transcription

1 Attitude Representation Basilio Bona DAUIN Politecnico di Torino Semester 1, B. Bona (DAUIN) Attitude Representation Semester 1, / 3

2 Mathematical preliminaries A 3D rotation matrix R = [ r 1 r r 3 ] = [rij ] contains only three independent parameters that represent the attitude (aka orientation) of the body-frame with respect to some fixed/inertial frame. We indicate these parameters as α = [ α 1 α α 3 ] T. they can be described in different ways: some consist in just three angles, with various names and meanings, other representations use more than three parameters, but add some constraints that reduce to three the number of independent parameters. Each representation has positive and negative characteristics. In the following we will introduce the most common ways to describe the attitude of a rigid body. B. Bona (DAUIN) Attitude Representation Semester 1, / 3

3 Direction cosines Direction cosines coincide with the 9 elements of the rotation matrix R itself. R contains in its columns r i (or rows) the representation of the unit basis vectors of the local frame with respect to the world frame; since this representation is a scalar product, and both vectors are unit vectors, the result is the cosine of the angle between the two basis vectors. This representation consists of nine parameters constrained by six orthogonality constraints; in a cartesian reference frame the angles are all 90, so that r i r j = 0 if i j and r i r i = 1, therefore only three free parameters result. The parameters [ α 1 α α 3 ] T are hidden in R but can be extracted using inverse trigonometric relations that depend on how α are chosen. B. Bona (DAUIN) Attitude Representation Semester 1, / 3

4 Axis-angle representation Euler s theorem states that the composition of any number of rotations is equivalent to a single rotation of an angle θ around an axis represented by the unit vector u = [ u 1 u u 3 ] T, u = 1. The axis-angle representation is given by the following rotation matrix: Rot(u,θ) = u 1 (1 c θ ) + c θ u 1 u (1 c θ ) u 3 s θ u 1 u 3 (1 c θ ) + u s θ u 1 u (1 c θ ) + u 3 s θ u (1 c θ ) + c θ u u 3 (1 c θ ) u 1 s θ u 1 u 3 (1 c θ ) u s θ u u 3 (1 c θ ) + u 1 s θ u3 (1 c θ ) + c θ where c θ cosθ s θ sinθ B. Bona (DAUIN) Attitude Representation Semester 1, / 3

5 Axis-angle representation Given the rotation matrix R, the rotation angle θ is computed as follows: ( ) tr (R) 1 θ = ±arccos If R R T, the unit vector u can be obtained building the skew-symmetrical matrix and computing the u i terms as follows S(u) 0 u 3 u u 3 0 u 1 = 1 (R R T) u u 1 0 sinθ If R = R T then S(u) = O; we have two cases: Case 1. R = R T = I Since sinθ = 0 ± kπ, θ is set to zero and u is undetermined, since no rotation occurs. B. Bona (DAUIN) Attitude Representation Semester 1, / 3

6 Axis-angle representation Case. R = R T I It follows that R T R = RR T = RR = R = I. This means that after two rotations of the same angle θ around the same axis u the orientation is given by the identity matrix, i.e., a null rotation; this implies θ = 0 ± π and θ = ±π. In order to find u, we compute S (u) as S = 1 1 cosθ ( R + R T and then we equate it to the symbolic relation S = uu T I = I ) u 1 1 u 1u u 1 u 3 u u 1 u 1 u u 3 u 3 u 1 u 3 u u3 1 B. Bona (DAUIN) Attitude Representation Semester 1, / 3

7 Cardan angles Cardan angles Each rotation matrix R can built by the composition of three elementary rotations R = Rot(u 1,α 1 )Rot(u,α )Rot(u 3,α 3 ) where u 1,u,u 3 are a proper choice of the unit vectors u l {i,j,k}. The only constraint is that u 1 u or u u 3, since Rot(u l,α 1 )Rot(u l,α ) = Rot(u l,α 1 + α ) leads to only one elementary rotation. The are twelve admissible combinations of rotations that form the set of Cardan angles. B. Bona (DAUIN) Attitude Representation Semester 1, / 3

8 Cardan angles 1) R(i,θ 1 )R(j,θ )R(k,θ 3 ) RPY angles ) R(i,θ 1 )R(k,θ 3 )R(j,θ ) 3) R(j,θ )R(i,θ 1 )R(k,θ 3 ) 4) R(j,θ )R(k,θ 3 )R(i,θ 1 ) 5) R(k,θ 3 )R(i,θ 1 )R(j,θ ) 6) R(k,θ 3 )R(j,θ )R(i,θ 1 ) RPY angles 1 7) R(i,α)R(j,β)R(i,γ) 8) R(i,α)R(k,β)R(i,γ) 9) R(j,α)R(i,β)R(j,γ) 10) R(j, α)r(k, β)r(j, γ) 11) R(k, α)r(i, β)r(k, γ) Euler angles 1 1) R(k, α)r(j, β)r(k, γ) Euler angles B. Bona (DAUIN) Attitude Representation Semester 1, / 3

9 Euler angles Among the Cardan angles, the most familiar are the Euler angles (ϕ, θ, ψ), that have the following (implicit) parametrization: R φ,θ,ψ = R(φ,θ,ψ) = R z,φ R x,θ R z,ψ = 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 θ B. Bona (DAUIN) Attitude Representation Semester 1, / 3

10 Euler angles Figure: Euler angles. B. Bona (DAUIN) Attitude Representation Semester 1, / 3

11 Euler angles To compute the Euler angles from a generic rotation matrix defined as: R = r 11 r 1 r 13 r 1 r r 3 r 31 r 3 r 33 where the elements r ij are known, it is necessary to solve the following non linear system of equations: r 11 = c φ c ψ s φ c θ s ψ r 1 = c φ s ψ s φ c θ c ψ r 13 = s φ s θ r 1 = s φ c ψ + c φ c θ s ψ r = s φ s ψ + c φ c θ c ψ r 3 = c φ s θ r 31 = s θ s ψ r 3 = s θ c ψ r 33 = c θ B. Bona (DAUIN) Attitude Representation Semester 1, / 3

12 Euler angles The solution is obtained as follows φ = atan(r 13, r 3 ) ± kπ ψ = atan ( c φ r 1 s φ r, c φ r 11 + s φ r 1 ) ± kπ θ = atan ( s φ r 13 c φ r 3, r 33 ) ± kπ it is important to use the inverse trigonometric function atan(y, x) ( y ) θ = atan(y,x) = tan 1 = x = 0 θ 90 se x 0; y 0 90 θ 180 se x 0; y θ 90 se x 0; y 0 90 θ 0 se x 0; y 0 since it is more robust from a numerical point of view B. Bona (DAUIN) Attitude Representation Semester 1, / 3

13 Roll-Pitch-Yaw angles Another rather common parametrization id based on the Roll, Pitch, Yaw (RPY) angles (θ x,θ y,θ z ), also known as Tait-Bryant angles, defined as follows R θx,θ y,θ z = R(θ x,θ y,θ z ) = R z,θz R y,θy R x,θx = R(k,θ z )R(j,θ y )R(i,θ x ) c θ z c θy c θy s θz s θx s θy c θz c θx s θz s θx s θy s θz + c θx c θz c θx s θy c θz + s θx s θz c θx s θy s θz s θx c θz s θy s θx c θy c θx c θy B. Bona (DAUIN) Attitude Representation Semester 1, / 3

14 Roll-Pitch-Yaw angles Figure: RPY angles. B. Bona (DAUIN) Attitude Representation Semester 1, / 3

15 RPY angles The RPY angles are obtained from a generic rotation matrix as: θ x = atan(r 3, r 33 ) ± kπ θ z = atan( c θx r 1 + s θx r 13, c θx r s θx r 3 ) ± kπ θ y = atan( r 31, s θx r 3 + c θx r 33 ) ± kπ Other RPY definitions are possible and found in technical literature, as R θz,θ y,θ x = R(θ z,θ y,θ x ) = R x,θx R y,θy R z,θz = R(i,θ x )R(j,θ y )R(k,θ z ) c θy c θz c θy s θz s θy s θx s θy c θz + c θx s θz s θx s θy s θz + c θx c θz s θx c θy c θx s θy c θz + s θx s θz c θx s θy s θz + s θx c θz c θx c θy B. Bona (DAUIN) Attitude Representation Semester 1, / 3

16 R = eul = Reul = rpy = Rrpy = B. Bona (DAUIN) Attitude Representation Semester 1, / 3

17 Cardan angles singularities The sequence of three angles produces a singularity when a certain pattern of angles appears. For the first group of cardan angles the presence of a ±90 angle may produce a singular behaviour, since 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(k,90 ) = R(k,90 )R(i,θ y ) R(j,θ y )R(i, 90 ) = R(i, 90 )R(k,θ 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 ) (1) The problem of gimbal lock is due to these singularities. For the second group of cardan angles it is sufficient that the middle rotation matrix is equal to the identity matrix to give origin to a singular representation. B. Bona (DAUIN) Attitude Representation Semester 1, / 3

18 Singularities Euler angles When r 33 = c θ = ±1 the matrix R(φ,θ,ψ) reduces to that is function of a single angle. R(k,φ)R(k,ψ) = R(k,(φ + ψ)) In this case we say that the Euler representation is singular; of the three possible angles we have only θ = 0 ± π and γ = (φ + ψ); we cannot compute separately the two angles φ and ψ, but only their sum. This situation is described saying that θ does not decouple the other two rotations and a singular configuration results. B. Bona (DAUIN) Attitude Representation Semester 1, / 3

19 Singularities RPY angles Roll-Pitch-Yaw angles are subject to singularity too, as the following relations hold R(k,θ z )R(j,90 ) = R(j,90 )R(i,θ z ) () R(i,θ x )R(j,90 ) = R(j,90 )R(k,θ x ) (3) When θ y = 90 the zyx RPY matrix becomes singular, since 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 xyz RPY matrix becomes singular, since 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 now functions of only a combination of two angles; this fact is the cause of the so called gimbal-lock problem that occurs in gyroscopes and is well known since the incident on the Apollo 10 Manned Lunar Spacecraft. B. Bona (DAUIN) Attitude Representation Semester 1, / 3

20 Euler parameters Given a rotation matrix R(u,θ), of angle θ around u = [ u 1 u ] T, u 3 Euler parameters (v 1,v,v 3,v 4 ) are defined as follows: v 1 = u 1 sin θ, v = u sin θ, v 3 = u 3 sin θ, v 4 = cos θ they shall not be confused with the Euler angles Only three parameters are independent since: 4 i=1 v i = 1 B. Bona (DAUIN) Attitude Representation Semester 1, / 3

21 Euler parameters Given the Euler parameters v i, the corresponding rotation matrix is: R(v) = v 1 v v 3 + v 4 (v 1 v v 3 v 4 ) (v 1 v 3 + v v 4 ) (v 1 v + v 3 v 4 ) v1 + v v 3 + v 4 (v v 3 v 1 v 4 ) (v 1 v 3 v v 4 ) (v v 3 + v 1 v 4 ) v1 v + v 3 + v 4 Given the rotation matrix R = [r ij ], the correponsing Euler parameters v i are: v 4 = ± 1 (1 + tr R) sign ambiguity cancelled if π θ π v 1 = 1 4v 4 (r 3 r 3 ) v = 1 4v 4 (r 13 r 31 ) v 3 = 1 4v 4 (r 1 r 1 ) B. Bona (DAUIN) Attitude Representation Semester 1, / 3

22 Euler parameters Euler parameters can be computed from the Euler angles as follows: ( ) v 1 = sin φ ψ sin ( ) ( ) θ v = cos φ ψ sin ( ) θ ( ) v 3 = sin φ+ψ cos ( ) ( ) θ v 4 = cos φ+ψ cos ( ) θ The sign ambiguity in v 4 can be cancelled assuming the constraint: π θ π, or π θ π; in this way the parameter v 4 can only be positive. The rotation angle is computed as cosθ = v4 (v 1 + v + v 3 ) and the unit vector u as 1 u = v 1 v sin(θ/) v 3 B. Bona (DAUIN) Attitude Representation Semester 1, / 3

23 Euler parameters Euler parameters parametrization is very convenient: it is more compact than the rotation matrix R, and numerically more robust than the Euler angles, since to compute R = R(v) we do not need trigonometric functions. Moreover, given two rotations R(v a ) and R(v b ), the Euler parameters of the product R(v c ) = R(v a )R(v b ) are computed by the following matrix product (see also the matrix form of the quaternion product): v a4 v a3 v a v a1 v v c = F(v a )v b = a3 v a4 v a1 v a v a v a1 v a4 v v b a3 v a1 v a v a3 v a4 B. Bona (DAUIN) Attitude Representation Semester 1, / 3

24 Quaternions See the slides devoted to QUATERNIONS B. Bona (DAUIN) Attitude Representation Semester 1, / 3

25 Quaternions The unit quaternion u = (u 0,u 1,u,u 3 ) = (u 0,u) = cosθ + usinθ represents the rotation Rot(u,θ) around the axis specified by the unit vector u = [ u 1 u u 3 ] T Quaternions and elementary rotations R(i,α) u x = (cosα/, sinα/, 0, 0) R(j,β) u y = (cosβ/, 0, sinβ/, 0) R(k,γ) u z = (cosγ/, 0, 0, sinγ/) The vectorial base of quaternions correspond to elementary rotations of π around the principal axes i = (0,1,0,0) R(i,π) j = (0,0,1,0) R(j,π) k = (0,0,0,1) R(k,π) B. Bona (DAUIN) Attitude Representation Semester 1, / 3

26 Rotation vectors Instead of using the Euler parameters or the quaternions, that require four parameters and one constraint, we can use the so called rotation vectors r, whose r i components describe the rotation axis and whose norm r provides the rotation angle, or a trigonometric function of the angle. In general the rotation vector r is defined as r = f (θ)u where u is the unit vector characterizing the rotation axis and r = f (θ) is an odd function. B. Bona (DAUIN) Attitude Representation Semester 1, / 3

27 Rodrigues vectors The most common f (θ) odd functions are: a) f (θ) = θ b) f (θ) = sinθ c) f (θ) = sin θ c) f (θ) = tan θ The choice c) defines the Euler (rotation) vector r E = sin θ [ ] T u1 u u 3 not to be confused with Euler parameter or Euler angles. Euler rotation vectors have a strict relation with quaternions. B. Bona (DAUIN) Attitude Representation Semester 1, / 3

28 Rodrigues vectors The choice d) defines the Rodrigues (or Gibbs) rotation vector r R = tan θ [ u1 u u 3 ] T Rodrigues vector r R = [ r 1 r ] T r 3 and Euler parameters (v1,v,v 3,v 4 ) are related as follows: r 1 = v 1 v 4, r = v v 4, r = v 3 v 4 Rodrigues vectors are undefined for angles θ = (k ± 1)π. B. Bona (DAUIN) Attitude Representation Semester 1, / 3

29 Rodrigues vectors Given the rotation matrix R, the Rodrigues vector r R = [ r 1 r ] T r 3 is obtained from the elements of a skew-symmetric matrix S(r) S(r) = 0 r 3 r r 3 0 r 1 = R RT r r tr (R) Given the Rodrigues vector r R = [ r 1 r ] T, r 3 the rotation matrix R is computed as r1 r r 3 (r 1 r r 3 ) (r 1 r 3 + r ) 1 R = (1 + r1 + r + r 3 ) (r 1 r + r 3 ) r1 + r r 3 (r r 3 r 1 ) (r 1 r 3 r ) (r r 3 + r 1 ) r1 r + r 3 B. Bona (DAUIN) Attitude Representation Semester 1, / 3

30 Cayley-Klein parameters Cayley-Klein (CK) parameters are elements of complex matrices ( ) α β Q = γ δ where α,β,γ,δ are complex variables. Q is be unitary and the CK parameters obey the following constraints: α = δ β = γ Q can also be written as ( ) α β Q = β α with the additional constraint detq = αα + ββ = 1 B. Bona (DAUIN) Attitude Representation Semester 1, / 3

31 Cayley-Klein parameters Q contains three free parameters that can be used to describe rotations; given the CK parameters (α,β,γ,δ), it is possible to compute the rotation matrix as follows: 1 (α β γ + δ 1 ) j( α β + γ + δ ) γδ αβ R = 1 j(α β + γ δ 1 ) (α + β + γ + δ ) j(αβ + γ + δ) βδ αγ j(αγ + βδ) αδ + βγ R elements are all real, although they derive from complex numbers. Given the Euler angles φ,θ,ψ, the CK parameters can be computed as follows: α = e j(φ+ψ) cos ( ) θ β = je j(φ ψ) cos ( ) θ γ = je j(φ ψ) cos ( θ ) δ = e j(φ+ψ) cos ( θ ) B. Bona (DAUIN) Attitude Representation Semester 1, / 3

32 Cayley-Klein parameters The relation between CK parameters {α, β}, Euler parameters {v 1,v,v 3,v 4 }, and quaternion components {q 0,q 1,q,q 3 }, is the following: Q matrix can also be written as: α = q 0 + jq 3 = v 4 + jv 3 β = q + jq 1 = v + jv 1 Q = q j(q 1 σ 1 + q σ + q 3 σ 3 ) where 1 is the identity matrix and the σ i are the so-called Pauli spin matrices. [ ] [ ] [ ] j 1 0 σ 1 = σ 1 0 = σ j 0 3 =. 0 1 B. Bona (DAUIN) Attitude Representation Semester 1, / 3