5460978150

czoną przez osie Xq, Y₀, stąd widzimy, że

(2.6)

czyli

= 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

	'Pcx'		Px	— k cos <71
pc =	p ^1 cy	=	Py	— k sin <7i
	Pcz		Pz	— l4 sin 77

Aby wyznaczyć kąt q₃ wykorzystuje się twierdzenie cosinusów

(2.9)

d² = Zf + Z| — 2l₂l₃ cos (n — q₃).

Ponieważ zachodzi zależność cos {k — <73) — — cos 93, oraz dla dowolnej konfiguracji manipulatora jest spełniony związek d² = r² + (P_cz — Z1)², gdzie r² = Pl+P^, zatem otrzymuje się dalej

Pl + Pi + (Pc, - h)² =l²2+l²3 + 2/2/3COS<73,    (2.10)

i stąd

PŁ + Pl + (Pe>-h)²-il-ą . „

COS qz = ---7TT~,- = P-

2/2/3

(2.11)

Istnieją dwa rozwiązania powyższego równania, a mianowicie

, stąd 0 < <73 < 7T,    (2.12)

q.j = arccos -

^pl + ^pl + (^pcz ~ h)² - zi - /;

2/2/3

q._f = — arccos -

Pl+Pl + (Po-h?-ll-l?

2/2/3

stąd - 7r < <73 < 0.    (2.13)

Jednakże lepszym sposobem znalezienia kąta <73 jest zauważenie, że jeśli cos q₃ jest dany wzorem (2.11), to sin <73 jest dany odpowiednio wzorem

(2.14)

sin <73 = ±Vl -D²,

i stąd można wyznaczyć

<13 = Atan2(±'_l/1 - D², 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 <72 wyznacza się wyrażenia na kąty pomocnicz 5 oraz a zaznaczone na rysunku 2.2. Dla kąta 5 zachodzi

r

P_cz - h

(2.16)

12