2.

3 c¸c phÐp biÓu diÔn h−íng

Nguyễn Mạnh Tiến 2.3 Các phép biểu diễn hướng 1

 2.3 C¸c phÐp biÓu diÔn h−íng

Ma trËn quay ARB gåm 9 thµnh phÇn kh«ng ®éc lËp nhau

A
RB = [ A
iB , A jB , A k B ]
Trong thùc tÕ chØ cÇn dïng 3 tham sè ®éc lËp ®Ó biÓu diÔn h−íng cña
mét KTĐ trong KTĐ gốc .

Đã lµ : PhÐp biÓu diÔn h−íng tèi thiÓu – Minimal Representation

of orientation :

+ Euler

+ RPY

Nguyễn Mạnh Tiến 2.3 Các phép biểu diễn hướng 2

 2.3.1 Phép biểu diễn Euler

➊ Phép quay Euler : 3 phép quay so với KTĐ hiện tại :

• Rot (zA, Φ)
• Rot (y’A, θ) zA
z’’’A = z”A
• Rot (z” A, Ψ) Ψ
y ’’’A
Φ
θ
y ’A=y”A
yA

xA
x’A X’’’A
x”A

Nguyễn Mạnh Tiến 2.3 Các phép biểu diễn hướng 3

 2.4.1 Phép biểu diễn Euler

Xác định phép biến đổi ARB khi biết các gócΦ, θ, Ψ :
(2.3-1)
CϕCθCψ − SϕSψ −CϕCθSψ − SϕCψ CϕSθ 0
SϕCθCψ + CϕSψ −SϕCθSψ + CϕCψ SϕSθ 0 
A
RB = REuler =
 −SθCψ SθSψ Cθ 0
 
 0 0 0 1

Bài toán ngược : Xác định góc Φ, θ, Ψ khi biết ma trân ARB :

r11 r12 r13 0

r r22 r23 0  (2.3-2)
A
RB =  21

r31 r32 r33 0

 
0 0 0 1

Nguyễn Mạnh Tiến 2.3 Các phép biểu diễn hướng 4

 2.3.1 Phép biểu diễn Euler

So sánh (2.3-2) và (2.3-3) :

CΦCθCΨ - SΦSΨ = r11 (2.3-4a)
-CΦCθSΨ - SΦCΨ = r12 (2.3-4b)
CΦSθ = r13 (2.3-4c)
SΦCθCΨ + CΦSΨ = r21 (2.3-4d)
-SΦCθSΨ +CΦCΨ = r22 (2.3-4e)
SΦSθ = r23 (2.3-4f)
-SθCΨ = r31 (2.3-4g)
SθSΨ = r32 (2.3-4h)
-Cθ = r33 (2.3-4i)

Nguyễn Mạnh Tiến 2.3 Các phép biểu diễn hướng 5

 2.3.1 Phép biểu diễn Euler

Từ (2.3-4c) và (2.3-4f) : Sθ = ± r132 + r232 (2.3-5)

Kết hợp (2.3-5) – (2.3-4i) : (

θ = a tan 2 ± r132 + r232 , r33 ) (2.3-6)
Nếu Sθ ≠ 0
(2.3-4c) và (2.3-4f) : (
ϕ = a tan 2 ± r23 , ± r13 ) (2.3-7)
(2.3-4c) và (2.3-4f) : ψ = a tan 2(± r 32 ,∓ r31 ) (2.3-8)
0<θ<π : π<θ<2π :
(
θ = a tan 2 r132 + r232 , r33 ) (
θ = a tan 2 − r132 + r232 ,−r33 )
(
φ = a tan 2 r23 , r33 ) (2.3-9) (
φ = a tan 2 − r23 ,−r33 ) (2.3-10)
ψ = a tan 2(r 32 ,−r31 ) ψ = a tan 2(− r , r31 )
32

Nếu Sθ = 0 : Φ bất kỳ
θ = 900- r33.900 và Ψ = atan2(r21, r22)-r33.Φ

Nguyễn Mạnh Tiến 2.3 Các phép biểu diễn hướng 6

 2.3.2 Phép biểu diễn RPY (Roll-Pitch-Yaw)

➊ Phép quay RPY : 3 phép quay so với KTĐ gốc :

• Rot (xA, Ψ)
• Rot (yA, θ) zA
• Rot (zA ,Φ)
Φ

θ
yA
Ψ
xA

Nguyễn Mạnh Tiến 2.3 Các phép biểu diễn hướng 7

 2.3.2 Phép biểu diễn RPY (Roll-Pitch-Yaw)

Xác định phép biến đổi ARB khi biết các gócΦ, θ, Ψ :

CϕCθ CϕSθSψ − SϕCψ CϕSθCψ + SϕSψ 0

 SϕCθ SϕSθSψ + CϕCψ SϕSθCψ − CϕSψ 0  (2.3-11)
A
RB = RRPY =
 −Sθ CθSψ CθCψ 0
 
 0 0 0 1

Bài toán ngược : Xác định góc Φ, θ, Ψ khi biết ma trân ARB :

r11 r12 r13 0

r r22 r23 0  (2.3-12)
A
RB =  21

r31 r32 r33 0

 
0 0 0 1

Nguyễn Mạnh Tiến 2.3 Các phép biểu diễn hướng 8

 2.3.2 Phép biểu diễn RPY (Roll-Pitch-Yaw)

So sánh (2.3-11) và (2.3-12) :

CΦCθ = r11 (2.3-13a)
SΦCθ = r12 (2.3-13b)
-Sθ = r13 (2.3-13c)
CΦSθSΨ - SΦCΨ = r21 (2.3-13d)
SΦSθSΨ + CΦCΨ = r22 (2.3-13e)
Cθ SΨ = r23 (2.3-13f)
CΦSθCΨ + SΦSΨ = r31 (2.3-13g)
SΦSθCΨ - CΦCΨ = r32 (2.3-13h)
CθCΨ = r33 (2.3-13i)

Nguyễn Mạnh Tiến 2.3 Các phép biểu diễn hướng 9

 2.3.2 Phép biểu diễn RPY (Roll-Pitch-Yaw)

-π /2<θ<π/2 : π /2<θ<3π/2 :

(
φ = a tan 2 r21 , r11 ) (
φ = a tan 2 − r21 ,−r11 )
( )
θ = a tan 2 − r31 , r322 + r332 (2.3-14) (
θ = a tan 2 − r31 ,− r322 + r332 , )(2.3-15)
ψ = a tan 2 r32 , r33( ) (
ψ = a tan 2 − r32 ,−r33 )

Nguyễn Mạnh Tiến 2.3 Các phép biểu diễn hướng 10