Orientacja z atan2

Zadanie
Dla macierzy orientacji manipulatora FANUC R-2000iB postaci


b1 c 1 − b2 s 1
b3 c 1 − b4 s 1
b5 c 1 + s 4 s 5 s 1


b
s
+
b
c
b
s
+
b
c
b
R(q) =  1 1
2 1
3 1
4 1
5 s 1 − s 4 s 5 c1  ,
b6 c6 − c3 s4 s6 −b6 s6 − c3 s4 c6 −s3 c5 − c3 c4 s5
(1)
gdzie b1 = −c4 c5 c6 s3 + s4 s6 s3 − c3 c6 s5 , b2 = c5 c6 s4 + c4 s6 , b3 = c6 s3 s4 + c4 c5 s3 s6 +
c3 s5 s6 , b4 = c4 c6 −c5 s4 s6 , b5 = c4 s3 s5 −c3 c5 , b6 = c3 c4 c5 −s3 s5 , oraz q = (q1 , . . . , q6 )T ,
wyznaczyć kąty KKM zdefiniowane jako KKM(φ, θ, ψ + π).
Didaskalia
Funkcja atan21 :
x
α = atan2(x, y) = arctan
y
!
sin α
= arctan
,
cos α
x y
+ +
+ −
− −
− +
ćwiartka α
pierwsza
druga
trzecia
czwarta
(2)
Rozwiązanie
Macierz rotacji dla wybranych kątów:
KKM(φ, θ, ψ + π) = Rot(Z, φ)Rot(Y , θ)Rot(X , ψ + π) =


cθ cφ sφ cψ − cφ sθ sψ −sφ sψ − cφ sθ cψ


= cθ sφ −cφ cψ − sφ sθ sψ cφ sψ − sφ sθ cψ  . (3)
−sθ
−cθ sψ
−cθ cψ
Stąd
 


ψ
atan2(−b6 s6 − c3 s4 c6 , −c5 s3 − c3 c4 s5 )
 


− arcsin(b6 c6 − c3 s4 s6 )
θ = 
.
φ
atan2(b1 s1 + b2 c1 , b1 c1 − b2 s1 )
1
Dla wartości argumentów 0 wartość funkcji atan2 jest nieokreślona.
1
(4)