Euler Angles and Rotation Matrix

Rotation Matrix

Direction Cosine Matrix

({{n, o, a}}) ({{x, y, z}})

unit column vector

n = ({{n_x, n_y, n_z}}) 

o = ({{o_x, o_y, o_z}}) 

a = ({{a_x, a_y, a_z}}) 

R = ({{n_x, o_x, a_x}, {n_y, o_y, a_y}, {n_z, o_z, a_z}})

n . o = 0

o . a = 0

a . n = 0

| n | = 1

| o | = 1

| a | = 1

Orthogonal Matrix

RR = I

R = R^(-1)

ZYX (Roll-Pitch-Yaw) Angles

X - axis (Yaw) φ→R_x[φ]

Y - axis (Pitch) θ→R_y[θ]

z - axis (Roll) ψ→R_z[ψ]

R_zyx = R_z[ψ] R_y[θ] R_x[φ]

XYZ Angles

R_xyz = R_x[φ] R_y[θ] R_z[ψ]

Rotation Matrix and Small Angels

Cos[α] ≈1, Sin[α] ≈α

R_zyx = R_z[ψ] R_y[θ] R_x[φ]

( {{1, -ψ, 0}, {ψ, 1, 0}, {0, 0, 1}} ) . ( {{1, 0, θ}, {0, 1, 0}, {-θ, 0, 1}} ) . ( {{1, 0, 0}, {0, 1, -φ}, {0, φ, 1}} )//MatrixForm

R_xyz = ( {{1, 0, 0}, {0, 1, -φ}, {0, φ, 1}} ) . ( {{1, 0, θ}, {0, 1, 0}, {-θ, 0, 1}} ) . ( {{1, -ψ, 0}, {ψ, 1, 0}, {0, 0, 1}} )//MatrixForm

Rotation Matrix to Euler Angles

R = ({{r_11, r_12, r_13}, {r_21, r_22, r_23}, {r_31, r_32, r_33}})

ZYX (Roll-Pitch-Yaw) Angles

θ={-π/2,π/2}

ψ = ArcTan[r_11, r_21]

θ = ArcSin[-r_31]

φ = ArcTan[r_33, r_32]

θ={π/2,3π/2}

ψ = ArcTan[-r_11, -r_21]

θ = ArcSin[-r_31]

φ = ArcTan[-r_33, -r_32]

θ (Pitch)=π/2, Cos[θ]≈0

r_23 - r_12 = (Sin[θ] + 1) Sin[ψ - φ]

r_13 + r_22 = (Sin[θ] + 1) Cos[ψ - φ]

r_23 + r_12 = (Sin[θ] - 1) Sin[ψ + φ]

r_13 - r_22 = (Sin[θ] + 1) Cos[ψ + φ]

ψ = ArcTan[r_13 - r_22, r_23 + r_12]

θ = ArcSin[-r_31]

φ = ψ - ArcTan[r_13 + r_22, r_23 - r_12]

Euler Angle

[Graphics:HTMLFiles/index_44.gif]

C_n^b = C_3^b[φ] C_2^3[θ] C_n^2[ψ]

R_zyx = R_z[ψ] R_y[θ] R_x[φ]

Derivation

Kalman Filter

System Model

C_1 = -Sin[θ]

C_2 = Sin[φ] Cos[θ]

C_3 = Cos[φ] Cos[θ]

Overscript[C, .] _1 = -Overscript[θ, .] Cos[θ] = ω_zC_2 - ω_yC_3

Overscript[C, .] _2 = Overscript[φ, .] Cos[φ] Cos[θ] - Overscript[θ, .] Sin[φ] Sin[θ] = -ω_zC_1ω_xC_3

Overscript[C, .] _3 = -Overscript[φ, .] Sin[φ] Cos[θ] - Overscript[θ, .] Cos[φ] Sin[θ] = ω_yC_1 - ω_xC_2

({{Overscript[C, .] _1}, {Overscript[C, .] _2}, {Overscript[C, .] _3}}) = ({{0, ω_z, -ω_y}, {-ω_z, 0, ω_x}, {ω_y, -ω_x, 0}}) ({{C_1}, {C_2}, {C_3}}) + W

≈ -g ({{-Sin[θ]}, { Sin[φ] Cos[θ]}, { Cos[φ] Cos[θ]}}) = -g ({{C_1}, {C_2}, {c_3}})

({{f_x}, {f_y}, {f_z}}) = -g ({{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}) . ({{C_1}, {C_2}, {C_3}}) + V

Analysis on Position And Orientation

Cartesian coordinate (직교좌표계)

unit vector

[Graphics:HTMLFiles/index_68.gif]

One Point P Vector

P = P·Overscript[x,^] + P·Overscript[y,^] + P·Overscript[z,^] = p_xOverscript[x,^] + p_yOverscript[y,^] + p_zOverscript[z,^]

Direction Vector : P = ({{p_x}, {p_y}, {p_z}})

위치 표현

[Graphics:HTMLFiles/index_71.gif]

좌표계 {A}, {B} 에서 본 점 P의 위치

Overscript[x,^] _B, Overscript[y,^] _B, Overscript[z,^] _B를 좌표계 {A} 의 좌표성분으로 표시하면

Overscript[x,^] _B = Overscript[x,^] _B·Overscript[x,^] _A + Overscript[x,^] _B·Overscript[y,^] _A + Overscript[x,^] _B·Overscript[z,^] _A

Overscript[x,^] _B = ({{Overscript[x,^] _B·Overscript[x,^] _A}, {Overscript[x,^] _B·Overscript[y,^] _A}, {Overscript[x,^] _B·Overscript[z,^] _A}})

좌표계 {A} 에서 본 벡터의 방향 , Direction Vector (방향 벡터)

Dirction Cosine (∴단위벡터의 내적→두 벡터 사이 가의 코사인 값)

v = v_xe_x + v_ye_y + v_ze_z

α = Cos[a] = (v·e_x)/(| v |) = v_x/(v_x^2 + v_y^2 + v_z^2)^(1/2)

β = Cos[b] = (v·e_y)/(| v |) = v_y/(v_x^2 + v_y^2 + v_z^2)^(1/2)

γ = Cos[c] = (v·e_z)/(| v |) = v_z/(v_x^2 + v_y^2 + v_z^2)^(1/2)

Cos[a]^2 + Cos[b]^2 + Cos[c]^2 = 1

상대 개념 위치

상대위치표현 ^AP, ^BP의 표현

P = ^AP = ^Ap_xOverscript[x,^] _A + ^Ap_yOverscript[y,^] _A + ^Ap_zOverscript[z,^] _A

= ^BP = ^Bp_xOverscript[x,^] _B + ^Bp_yOverscript[y,^] _B + ^Bp_zOverscript[z,^] _B

Rotation Matrix _B^AR

Overscript[x,^] _B = Overscript[x,^] _B·Overscript[x,^] _A + Overscript[x,^] _B·Overscript[y,^] _A + Overscript[x,^] _B·Overscript[z,^] _A

Overscript[y,^] _B = Overscript[y,^] _B·Overscript[x,^] _A + Overscript[y,^] _B·Overscript[y,^] _A + Overscript[y,^] _B·Overscript[z,^] _A

Overscript[z,^] _B = Overscript[z,^] _B·Overscript[x,^] _A + Overscript[z,^] _B·Overscript[y,^] _A + Overscript[z,^] _B·Overscript[z,^] _A

{A} 에서 본 {B} 의 표현식

물체의 방위 (상대적 회전량) 를 나타냄

^AP = _B^AR . ^BP

{B} 좌표계 성분을 {A} 좌표계 성분으로 변환

^AP = _B^AR . ^BP

회전전행렬 _B^AR을 통하여 변환됨을 의미

⇒_B^AR : Transformation Matrix (변환행렬) 라고 정의

좌표계 {A} 에 대한 좌표계 {B} 의 회전행렬

방위의 표현: 회전행렬

[Graphics:HTMLFiles/index_104.gif]

= ({{Cos[α], -Sin[α], 0}, {Sin[α], Cos[α], 0}, {0, 0, 1}})

= Rot (Overscript[z,^], α)

Rot (Overscript[z,^], ψ) = ( {{Cos[ψ], -Sin[ψ], 0}, {Sin[ψ], Cos[ψ], 0}, {0, 0, 1}} )

Rot (Overscript[y,^], θ) = ( {{Cos[θ], 0, Sin[θ]}, {0, 1, 0}, {-Sin[θ], 0, Cos[θ]}} )

Rot (Overscript[x,^], φ) = ( {{1, 0, 0}, {0, Cos[φ], -Sin[φ]}, {0, Sin[φ], Cos[φ]}} )

회전행렬의 성질 (_A^BR=_B^AR^(-1)=_B^AR)

좌표계 {B} 에 대한 좌표계 {A} 의 회전행렬 : _A^BR

좌표계 {A} 에 대한 좌표계 {B} 의 회전행렬 : _B^AR

_A^BR = _B^AR

{{{^AP = _B^AR . ^BP}, {^BP = _A^BR . ^AP}} ⇒ ^AP = _B^AR . ^BP = _B^AR . _A^BR . ^AP ⇒ _B^AR . _A^BR = I

_A^BR = _B^AR^(-1) = _B^AR

동차변환을 이용한 좌표변환

^AP = ^Ar_B + _B^AR . ^BP

방위가 서로 다르고 원점이 ^Ar_B만큼 떨어진 좌표계 {A} 와 {B}

[Graphics:HTMLFiles/index_124.gif]

^AP = ^Ar_B + ^BP (incorrect !)

^AP = ^Ar_B + _B^AR . ^BP (correct !)

^Ar_B : 두 좌표계 사이의 상대거리 또는 전위 또는 병진 (Translation)

_B^AR : 두 좌표계 사이의 상대회전 (Rotation)

({{^AP}, {1}}) = ({{_B^AR, ^Ar_B}, {{{0, 0, 0}}, 1}}) . ({{^BP}, {1}})

^AP = _B^AT . ^BP

_B^AT : 동차 변환행렬 (homogeneous transformation matrix)

동차 변환행렬의 구조와 표현

_B^AT = ({{_B^AR, ^Ar_B}, {{{0, 0, 0}}, 1}})

T = Trans[p_x, p_y, p_z] = ({{1, 0, 0, ^Ap_x}, {0, 1, 0, ^Ap_y}, {0, 0, 1, ^Ap_z}, {0, 0, 0, 1}})

_B^AT = ({{Cos[α], -Sin[α], 0, ^Ap_x}, {Sin[α], Cos[α], 0, ^Ap_y}, {0, 0, 1, ^Ap_z}, {0, 0, 0, 1}})

= ({{1, 0, 0, ^Ap_x}, {0, 1, 0, ^Ap_y}, {0, 0, 1, ^Ap_z}, {0, 0, 0, 1}}) . ({{Cos[α], -Sin[α], 0, 0}, {Sin[α], Cos[α], 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}})

= Trans[p_x, p_y, p_z] . Rot[z_A, α]

_B^AT = Trans[p_x, p_y, p_z] . Rot[z_A, α]

주의→ _B^AT≠Rot[z_A, α] . Trans[p_x, p_y, p_z]

역변환 (Inverse Transformation)

inverse transformation for a given  _B^AT ⇒ _A^BT

^AP = _B^AT . ^BP ⇔ ^BP = _A^BT . ^AP = (_B^AT)^(-1) . ^AP

_A^BT = (_B^AT)^(-1)

유도

^AP = ^Ar_B + _B^AR . ^BP

_B^AR . ^AP = _B^AR . ^Ar_B + _B^AR . _B^AR . ^BP

_B^AR . ^AP = _B^AR . ^Ar_B + ^BP

^BP = _B^AR . ^Ar_B - _B^AR . ^AP

({{^BP}, {1}}) = ({{_B^AR, -_B^AR . ^Ar_B}, {{{0, 0, 0}}, 1}}) . ({{^AP}, {1}})

^BP = _A^BT . ^AP

_A^BT = (_B^AT)^(-1) = ({{_B^AR, -_B^AR . ^Ar_B}, {{{0, 0, 0}}, 1}})

변환의 합성 (_C^AT=_B^AT._C^BT)

_C^BT, _B^AT가 주어졌을 때 점P의 좌표계 {A} 에 대한 표현 ^AP

[Graphics:HTMLFiles/index_155.gif]

{{{^BP = _C^BT . ^CP}, {^AP = _B^AT . ^BP}}

^AP = _B^AT . ^BP = _B^AT . _C^BT . ^CP = _C^AT . ^CP

_C^AT = _B^AT . _C^BT

변환 방정식: 페루프 이용으로 미지의 변환을 다른 변환들 사이의 관계를 통해서 얻을 수 있다.

연속좌표변환: 절대변환과 상대변환

이동좌표계 (moving frame)

변환 (회전 또는 병진) 에 의해 새롭게 얻어지는 좌표계

고정좌표계 (fixed frame)

변환에 관계없이 처음과 동일하게 고정된 좌표계

절대변환 (absolute transform)

상대변환 (relative transform)

좌표계{A}와{B} 사이의 변환을 나태내는 동차변환행렬

_B^AT = Trans[p_x, p_y, p_z] . Rot[z_A, α]

[Graphics:HTMLFiles/index_164.gif]

O: 이동좌표계를 따라 병진 후 이동좌표계에 대해 회전

[Graphics:HTMLFiles/index_165.gif]

X: 이동좌표계를 따라 회전 후 이동좌표계에 대해 병진

[Graphics:HTMLFiles/index_166.gif]

O: 고정좌표계를 따라 회전 후 고정좌표계에 대해 병진

[Graphics:HTMLFiles/index_167.gif]

X: 고정좌표계를 따라 병진 후 고정좌표계에 대해 회전

[Graphics:HTMLFiles/index_168.gif]

올바른 변환을 나타내는 조합

두가지 모두 동일한 결과 _B^AT를 나타냄

_B^AT = Trans[p_x, p_y, p_z] . Rot[α, β, γ]

∴상대변환의 구현 (post - multiply)

∴절대변환의 구현 (pre - multiply)

^Ar_D = ({{a}, {b}, {c}})

^Br_D = ({{a}, {b}, {c}}) = ^Br_D^'

^Ar_D^' = _B^AT . ^Br_D^'

^Ar_C^' = _B^AT . ^Br_C^'

방위의 다른 표현

방위의 표현

오일러 각 (Euler angle)과 롤-피키-요(Roll-Pitch-Yaw) 방법

각각 12가지 표현법 존재

Z-Y-Z 오일러 각

좌표계 {A} 의 Z축에 대해 α만큼 회전 후

_B^AT = Rot[Overscript[z,^], α] . Rot[Overscript[y,^], β] . Rot[Overscript[z,^], γ]

[Graphics:HTMLFiles/index_196.gif]

Z-Y-X 오일러 각

좌표계 {A} 의 Z축에 대해 α만큼 회전 후

_B^AT = Rot[Overscript[z,^], α] . Rot[Overscript[y,^], β] . Rot[Overscript[x,^], γ]

[Graphics:HTMLFiles/index_204.gif]

X-Y-Z (롤-피치-요)

고정기준좌표계 {A} 의 X축에 대해 γ만큼 회전 후

고정좌표계의 Y축에 대해 β만큼 회전한 후,

Z - Y - X 오일러와 동일

_B^AT = Rot[Overscript[z,^], α] . Rot[Overscript[y,^], β] . Rot[Overscript[x,^], γ]

[Graphics:HTMLFiles/index_213.gif]