Manipulator przeznaczony do celów dydaktycznych - Laboratorium ...

12
czoną przez osie X 0 , Y 0 , stąd widzimy, że
czyli
tan q 1 = P y
P x
, (2.6)
q 1 = Atan2(P y , P x ), (2.7)
gdzie Atan2(·, ·) oznacza dwuargumentową funkcję arcus tangens.
W celu ułatwienia obliczenia kolejnych zmiennych przegubowych wyznaczony został punkt
pomocniczy P c o współrzędnych
⎡ ⎤ ⎡
⎤
P cx P x − k cos q 1
⎢ ⎥ ⎢
⎥
P c = ⎣ P cy ⎦ = ⎣ P y − k sin q 1 ⎦ , gdzie k = l 4 cos η. (2.8)
P cz P z − l 4 sin η
Aby wyznaczyć kąt q 3 wykorzystuje się twierdzenie cosinusów
d 2 = l 2 2 + l 2 3 − 2l 2 l 3 cos (π − q 3 ). (2.9)
Ponieważ zachodzi zależność cos (π − q 3 ) = − cos q 3 , oraz dla dowolnej konfiguracji manipulatora
jest spełniony związek d 2 = r 2 +(P cz −l 1 ) 2 , gdzie r 2 = Pcx 2 +P cy 2 , zatem otrzymuje
się dalej
Pcx 2 + P cy 2 + (P cz − l 1 ) 2 = l2 2 + l2 3 + 2l 2l 3 cos q 3 , (2.10)
i stąd
cos q 3 = P 2 cx + P 2 cy + (P cz − l 1 ) 2 − l 2 2 − l2 3
2l 2 l 3
. = D. (2.11)
Istnieją dwa rozwiązania powyższego równania, a mianowicie
oraz
q 3 = arccos P 2 cx + P 2 cy + (P cz − l 1 ) 2 − l 2 2 − l2 3
2l 2 l 3
, stąd 0 q 3 π, (2.12)
q 3 = − arccos P 2 cx + P 2 cy + (P cz − l 1 ) 2 − l 2 2 − l 2 3
2l 2 l 3
, stąd − π q 3 0. (2.13)
Jednakże lepszym sposobem znalezienia kąta q 3 jest zauważenie, że jeśli cos q 3 jest dany
wzorem (2.11), to sin q 3 jest dany odpowiednio wzorem
i stąd można wyznaczyć
sin q 3 = ± √ 1 − D 2 , (2.14)
q 3 = Atan2(± √ 1 − D 2 , D). (2.15)
Zaletą tego drugiego sposobu jest rozróżnienie obu konfiguracji ”łokieć u góry” i ”łokieć
u dołu” przez wybór odpowiedniego znaku w równaniu (2.15).
W celu wyliczenia kąta q 2 wyznacza się wyrażenia na kąty pomocnicz δ oraz α zaznaczone
na rysunku 2.2. Dla kąta δ zachodzi
tan δ = P cz − l 1
r
= P cz − l 1
√ , (2.16)
P
2
cx − Pcy
2