Wstęp do Robotyki – c

W. Szynkiewicz, 2006

1

Kinematyka manipulatorów robotów

Podstawowe pojęcia:

• Ogniwo (człon, ramię) – bryła sztywna (zbiór punktów materialnych, których wzajemne położenie

jest stałe).

• Przegub (złącze) – ruchome połączenie dwóch ogniw; podstawowe typy przegubów: obrotowy

(rotacyjny) – R, przesuwny (translacyjny) – P, sferyczny (kulowy), cylindryczny, śrubowy, itd.

• Para kinematyczna – dwa człony połączone przegubem. W robotach najczęściej są pary kinematyczne

V klasy, tzn. takie, które mają jeden stopień swobody w przegubie. Spełniają ten warunek przegub
przesuwny (P) i rotacyjny (R).

• Łańcuch kinematyczny – zespół ogniw połączonych przegubami. Łańcuch może być otwarty albo

zamknięty (zawiera pętle kinematyczne).

• Manipulator – łańcuch sztywnych ogniw połączonych przegubami, mechanizm wieloczłonowy.

• Liczba stopni swobody manipulatora – zazwyczaj jest liczba przegubów (liczba niezależnych

napędów), jest to także najmniejsza liczba współrzędnych jednoznacznie opisująca konfigurację
manipulatora. Zwykle manipulator ma sześć stopni swobody. Manipulatory mające więcej niż sześć
stopni swobody nazywane są manipulatorami redundantnymi.

• Struktura kinematyczna manipulatora – struktura szeregowa, struktura równoległa, struktura

szeregowo-równoległa.

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

2

• Struktura geometryczna manipulatora – typ geometrii trzech głównych (początkowych od podstawy

robota) ogniw: stawowy, sferyczny, SCARA, cylindryczny, kartezjański.

• Kiść i końcówka robocza – kiść (zazwyczaj 2-3 ostatnie przeguby manipulatora), najczęściej jest

to tzw. kiść sferyczna, końcówka robocza – narzędzie, np. chwytak, końcówka spawalnicza, pistolet
lakierniczy, itp.

• Napędy manipulatora – napędy elektryczne, hydrauliczne, pneumatyczne.

• Więzy kinematyczne (ruchu) – więzy holonomicznie (zmniejszają liczbę zmiennych konfiguracyjnych

– liczbę stopni swobody) oraz więzy nieholonomiczne (ograniczenia na dopuszczalne prędkości – w
sensie kierunków i długości wektorów).

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

3

Przykładowe manipulatory robotów przemysłowych

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

4

Przegub przesuwny (P)

Przegub obrotowy (R)

Przegub sferyczny

Oznaczenia graficzne przegubu obrotowego i przesuwnego

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

5

Przegub

Cz³on

Koñcówka
(efektor)

Narzêdzie

Manipulator z przegubami obrotowymi

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

6

Manipulator stawowy (ABB IRB1400)

Przekroje przestrzeni roboczej manipulatora stawowego

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

7

Manipulator SCARA (Adept One)

Manipulator sferyczny (Stanford Arm)

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

8

Manipulator cylindryczny (Seiko RT3300)

Manipulator kartezjański (Epson)

W

s

tę

p

d

o

R

o

b

o

ty

k

i

–

c

W

.

S

z

y

n

k

ie

w

ic

z

,

2

0

0

6

9

P

rz

es

trz

en

ie

ro

b

o

cz

e

m

an

ip

u

la

to

ró

w

:

a)

sf

ery

cz

n

y,

b

)

S

C

A

R

A

,

c)

cy

li

n

d

ry

cz

n

y,

d

)

ka

rte

zj

ań

sk

i

W

s

tę

p

d

o

R

o

b

o

ty

k

i

–

c

W

.

S

z

y

n

k

ie

w

ic

z

,

2

0

0

6

10

Zestawienie struktur kinematycznych manipulatorów

Robot

Konstrukcja

Osie

Struktura

kinematyczna

Przestrzeñ

robocza

Przyk³ad

Typ

Kartezjañski
PPP

Cylindryczny
RPP

Sferyczny
RRP

SCARA
RRP

Stawowy
RRR

Równoleg³y

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

11

Opis kinematyczny układu robotycznego

Układ jest opisany współrzędnymi uogólnionymi

q

= [q

1

, q

2

, . . . , q

N

]

T

∈ R

N

należącymi do pewnego zbioru zwanego uniwersum konfiguracyjnym lub przestrzenią konfiguracyjną.

Prędkości uogólnione

˙

q

= [ ˙q

1

,

˙q

2

, . . . ,

˙q

N

]

T

∈ R

N

Przestrzeń R

N

× R

N

∼

= R

2N

położeń i prędkości uogólnionych jest nazywana uniwersum fazowym (jeśli

ruch nie podlega ograniczeniom jest to przestrzeń fazowa lub przestrzeń stanu).

Więzy ruchu:
Wzajemne związki między elementami układu, a także z otoczeniem opisuje się w postaci ograniczeń
(więzów) konfiguracyjnych

f

(q) = [f

1

(q), f

2

(q), . . . , f

k

(q)]

T

=

0

(1)

oraz ograniczeń (więzów) fazowych

A(q) ˙

q

=

0

(postać Pfaffa)

(2)

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

12

Zakładamy, że liczba więzów konfiguracyjnych jest k 6 N , zaś funkcje f

1

, f

2

, . . . , f

k

, są gładkie i

niezależne:

f

(q) =

0

⇒

rank

∂f

(q)

∂q

= k

(pełny rząd)

(3)

oraz, że macierz A(q) ma rozmiar l × N, l 6 N , jej elementy a

ij

(q) są gładkimi funkcjami i macierz ma

pełny rząd

rank A(q) = l

(4)

Jeśli nie ma ograniczeń fazowych (l = 0), to k niezależnych ograniczeń konfiguracyjnych określa rozmaitość
konfiguracyjną

Q =

q ∈ R

N

: f

1

(q) = f

2

(q) = . . . = f

k

(q) = 0

(5)

układu, której wymiar dim Q = N − k = n (gdzie n jest liczbą stopni swobody układu), z przestrzenią
styczną

T

q

Q = Ker

∂f

(q)

∂q

Ruch układu jest ograniczony do rozmaitości Q ⊂ R

N

.

Jeśli ograniczenia fazowe są holonomiczne tzn. można je scałkować i doprowadzić do postaci ograniczeń
konfiguracyjnych

h

(q) = [h

1

(q), h

2

(q), . . . , h

l

(q)]

T

=

0

(6)

wtedy prowadzi to do dalszego ograniczenia dopuszczalnych konfiguracji układu, gdyż ograniczenia
holonomiczne są ograniczeniami konfiguracyjnymi.

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

13

Kinematyką układu holonomicznego nazywamy odwzorowanie:

K

: Q −→ Z, z = K(q)

(7)

gdzie Z jest przestrzenią zadaniową lub rozmaitością zadaniową.

Kinematyka nie zawsze jest dana w sposób jawny, może też być wyrażona w postaci uwikłanej za
pośrednictwem ograniczeń konfiguracyjnych f (z, q) =

0.

Ograniczeń nieholonomicznych nie można scałkować, a ich występowanie nie zmniejsza osiągalności
konfiguracji, a jedynie może utrudniać sposób osiągania pewnych konfiguracji.

Załóżmy, że układ nie podlega więzom konfiguracyjnym (tj. k = 0, n = N ), wówczas z (

2

) wynika, że

∀q ∈ R

N

dopuszczalne prędkości muszą należeć do przestrzeni zerowej (jądra) macierzy A(q)

˙

q

∈ Ker A(q)

(8)

Niech G(q) = [g

1

(q), g

2

(q), . . . , g

n

−l

(q)] będzie macierzą, której wektory kolumnowe rozpinają przestrzeń

liniową Ker A(q) w konfiguracji q. Z niezależności ograniczeń fazowych wynika, że w każdym punkcie rząd
macierzy G(q) jest pełny,

rank G(q) = n − l = m

(9)

zaś własność (

8

) jest równoważna istnieniu wektora u ∈ R

m

(należącego do pewnej przestrzeni sterowań),

takiego że

˙

q

= G(q)u =

m

X

i

=1

g

i

(q)u

i

(10)

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

14

W pewnym otoczeniu punktu q więzy fazowe można wyrazić

[A

1

(q) A

2

(q)] ˙

q

=

0,

(11)

gdzie A

2

(q) jest rozmiaru l × l i ma pełny rząd.

A

−1

2

(q) A

1

(q) I

l

 ˙q = [W(q) I

l

] ˙

q

=

0,

co daje macierz G(q) =

"

I

N

−l

W(q)

#

, dzieląc współrzędne uogólnione na dwie grupy q =

q

1T

, q

2T



T

,

gdzie dim q

1

= N − l = m, dim q

2

= l i wówczas kinematyka układu ma postać

˙

q

1

= u,

˙

q

2

= W(q)u

(12)

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

15

Proste zadanie kinematyki dla manipulatora

• Manipulator składa się z ogniw (członów) sztywnych połączonych sztywnymi przegubami.

• Ogniwa połączone przegubem tworzą parę kinematyczną V klasy, czyli zakładamy, że przegub ma

jeden stopień swobody.

• Człony numerowane są od nieruchomej podstawy (człon 0), pierwszy człon ruchomy ma numer 1, aż

do ostatniego członu o numerze n.

• Notacja Denavita-Hartenberga (D-H). Do opisu kinematyki stosuje się parametry D-H. Dla

członu i podaje się wartości czterech parametrów (dwa pierwsze opisują sam człon, dwa kolejne
połączenie z sąsiednim członem):

– a

i

−1

– długość członu,

– α

i

−1

– skręcenie członu,

– d

i

– odsunięcie przegubu,

– θ

i

– kąt przegubu

• Przyjmujemy zasadę, że a

0

= a

n

= 0 i α

0

= α

n

0

= 0. Parametry d

i

oraz θ

i

są dobrze określone

dla przegubów od 2 do n − 1. Jeśli przegub 1 jest obrotowy/posuwisty, to można przyjąć dowolne
położenie zerowe dla θ

1

/d

i

, a d

1

= 0/θ

1

= 0. Tak samo postępujemy w przypadku członu n.

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

16

Rys. 1: Parametry Denavita-Hartenberga

• Związujemy sztywno z członem i układ współrzędnych i. Po związaniu z każdym członem układu

współrzędnych parametry D-H można zdefiniować następująco:

– a

i

−1

– odległość osi z

i

−1

od osi z

i

mierzona wzdłuż osi x

i

−1

,

– α

i

−1

– kąt między osiami z

i

−1

i z

i

mierzony wokół osi x

i

−1

,

– d

i

– odległość osi x

i

−1

od osi x

i

mierzona wzdłuż osi z

i

,

– θ

i

– kąt między osiami x

i

−1

i x

i

mierzony wokół osi z

i

,

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

17

• Należy znaleźć przekształcenie opisujące układ {i} względem układu {i − 1}, czyli macierz

i

−1

i

T

i

−1

i

T

= Rot

x,α

i

−1

Trans

x,a

i

−1

Rot

z,θ

i

Trans

z,d

i

=







1

0

0

0

0 cα

i

−1

−sα

i

−1

0

0 sα

i

−1

cα

i

−1

0

0

0

0

1













1 0 0 a

i

−1

0 1 0

0

0 0 1

0

0 0 0

1













cθ

i

−sθ

i

0 0

sθ

i

cθ

i

0 0

0

0

1 0

0

0

0 1













1 0 0 0
0 1 0 0
0 0 1 d

i

0 0 0 1







(13)

• Ostateczna postać macierzy

i

−1

i

T

jest następująca:

i

−1

i

T

=










cθ

i

−sθ

i

0

a

i

−1

sθ

i

cα

i

−1

cθ

i

cα

i

−1

−sα

i

−1

−sα

i

−1

d

i

sθ

i

sα

i

−1

cθ

i

sα

i

−1

cα

i

−1

cα

i

−1

d

i

0

0

0

1










,

gdzie s() ≡ sin() oraz c() ≡ cos()

(14)

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

18

x

x

x

z

x

x

y

x

y

z

x

z

z

q

q

q

q

1

q

q

q

3

3

2

42

4

0

0

0

1

2

3

3

5

5

5

4

6

6

1

'

'

Rys. 2: Układy związane z manipulatorem

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

19

Rys. 3: Manipulator robota Puma

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

20

Algorytm rozwiązywania prostego zadania kinematyki wykorzystującego notację D-H:

1. Umieść i oznacz osie przegubów z

1

, . . . , z

n

.

2. Przyjmij bazowy układ współrzędnych {0}, tak aby dla zerowej wartości współrzędnej konfiguracyjnej

osie układów {0} oraz {1} pokrywały się. Dla i = 1, . . . , 2 wykonaj kroki od 3 do 8.

3. Umieść początek układu O

i

w punkcie przecięcia osi z

i

przez wspólną normalną do osi z

i

oraz z

i

+1

lub w punkcie przecięcia osi z

i

oraz z

i

+1

gdy osie te przecinają się.

4. Wybierz oś x

i

wzdłuż prostej normalnej do osi z

i

oraz z

i

+1

lub w kierunku normalnej do płaszczyzny

obu tych osi gdy osie z

i

i z

i

+1

przecinają się.

5. Wybierz oś y

i

tak aby układ był prawoskrętny.

6. Wybierz takie usytuowanie układu {n} aby spowodować zerowanie się jak największej liczby

parametrów.

7. Utwórz tabelę parametrów D-H (a

i

−1

, α

i

−1

, d

i

, θ

i

).

8. Zbuduj macierze przekształcenia jednorodnego

i

−1

i

T

wstawiając parametry do równania (

14

)

9. Oblicz macierz

0

n

T

=

0

1

T

1

2

T . . .

n

−1

n

T

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

21

Przykład 1: Rozwiązać proste zadanie kinematyki dla płaskiego manipulatora trójczłonowego pokazanego
na rys.

4

.

y

0

x

0

y

1

y

2

y

3

x

1

x

2

L

2

L

1

x

3

{0}

q

1

q

2

q

3

Rys. 4: Manipulator płaski typu 3R

i

α

i

−1

a

i

−1

d

i

θ

i

1

0

0

0

θ

1

2

0

L

1

0

θ

2

3

0

L

2

0

θ

3

Tabela 1: Tablica parametrów Denavita-Hartenberga dla płaskiego manipulatora 3R

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

22

0

1

T

=










c

1

−s

1

0 0

s

1

c

1

0 0

0

0

1 0

0

0

0 1










1

2

T

=










c

2

−s

2

0 L

1

s

2

c

2

0 0

0

0

1 0

0

0

0 1










2

3

T

=










c

3

−s

3

0 L

2

s

3

c

3

0 0

0

0

1 0

0

0

0 1










Rozwiązaniem prostego zadania kinematyki jest macierz

0

4

T

=

0

1

T

1

2

T

2

3

T

3

4

T

Funkcje trygonometryczne sumy i różnicy kątów:

cos(θ

1

+ θ

2

) = c

12

= c

1

c

2

− s

1

s

2

sin(θ

1

+ θ

2

) = s

12

= c

1

s

2

+ s

1

c

2

cos(θ

1

− θ

2

) = c

1

c

2

+ s

1

s

2

sin(θ

1

− θ

2

) = s

1

c

2

− c

1

s

2

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

23

Rozwiązanie PZK dla robota typu 3R:

0

3

T

=










c

123

−s

123

0 L

1

c

1

+ L

2

c

12

s

123

c

123

0 L

1

s

1

+ L

2

s

12

0

0

1

0

0

0

0

1










(15)

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

24

Przykład 2:
Dla manipulatora przestrzennego typu 3R (rys.

5

) należy: związać z każdym członem układ współrzędnych

i przedstawić na rysunku. Podać w tabeli parametry D-H i obliczyć macierze jednorodne

i

−1

i

T

, a następnie

rozwiązać proste zadanie kinematyki dla tego manipulatora.

q

1

q

2

q

3

z

0, 1

y

2

x

0, 1

x

2

x

4

x

3

y

4

y

3

l

2

l

3

Rys. 5: Manipulator przestrzenny 3R

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

25

Rozwiązanie:

i

α

i

−1

a

i

−1

d

i

θ

i

1

0

0

0

θ

1

2

90

◦

0

0

θ

2

3

0

l

2

0

θ

3

4

0

l

3

0

−

Tabela 2: Tablica parametrów Denavita-Hartenberga (D-H) dla manipulatora przestrzennego typu 3R

0

1

T

=










c

1

−s

1

0 0

s

1

c

1

0 0

0

0

1 0

0

0

0 1










1

2

T

=










c

2

−s

2

0 a

1

0

0

−1 0

s

2

c

2

0

0

0

0

0

1










2

3

T

=










c

3

−s

3

0 l

2

s

3

c

3

0 0

0

0

1 0

0

0

0 1










3

4

T

=










1 0 0 l

3

0 1 0 0

0 0 1 0

0 0 0 1










Wstęp do Robotyki – c

W. Szynkiewicz, 2006

26

Rozwiązaniem prostego zadania kinematyki jest macierz

0

4

T

=

0

1

T

1

2

T

2

3

T

3

4

T

Funkcje trygonometryczne sumy i różnicy kątów:

cos(θ

1

+ θ

2

) = c

12

= c

1

c

2

− s

1

s

2

sin(θ

1

+ θ

2

) = s

12

= c

1

s

2

+ s

1

c

2

cos(θ

1

− θ

2

) = c

1

c

2

+ s

1

s

2

sin(θ

1

− θ

2

) = s

1

c

2

− c

1

s

2

Rozwiązanie PZK dla robota typu 3R:

0

4

T

=










c

1

c

23

−c

1

s

23

s

1

l

2

c

1

c

2

+ l

3

c

1

c

23

s

1

c

23

−s

1

s

23

−c

1

l

2

s

1

c

2

+ l

3

s

1

c

23

s

23

c

23

0

l

2

s

2

+ l

3

s

23

0

0

0

1










(16)

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

27

Odwrotne zadanie kinematyki (OZK) manipulatora

Przestrzeń robocza manipulatora jest to zbiór punktów, które końcówka robocza może osiągnąć. Aby
istniało rozwiązanie OZK zadany punkt docelowy dla końcówki musi należeć do przestrzeni roboczej.

Można wyróżnić dwa rodzaje przestrzeni roboczej:

• manipulacyjna przestrzeń robocza – jest to część przestrzeni roboczej, którą końcówka robocza

może osiągnąć z dowolną orientacją,

• osiągalna przestrzeń robocza – jest to część przestrzeni roboczej, którą końcówka robocza może

osiągnąć z co najmniej jedną orientacją.

Przykład 2: Przestrzeń robocza dla płaskiego manipulatora 2R.

y

0

x

0

L

2

L

1

L +L

1

2

|L L |

1

-

2

q

1

q

2

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

28

Dla danego manipulatora o n stopniach swobody z przestrzenią przegubową Q oraz przestrzenią zadaniową
Z ∈ SE(3) odwrotne zadanie kinematyki można wyrazić jako

k

−

1

: SE(3) ⊃ Z ∋ z → q = k

−

1

(z) ∈ Q

(17)

Jednakże, odwzorowanie odwrotne k

−1

może nie istnieć lub być niejednoznacznie określone.

Rozwiązanie odwrotnego zadania kinematyki:

Dane są wartości liczbowe elementów macierzy przekształcenia jednorodnego H

H

=

"

R d

0 1

#

opisujących położenie i orientację końcówki w układzie bazowym związanym z nieruchomą podstawą
robota. Należy znaleźć rozwiązanie (jedno lub wszystkie) równania macierzowego

0

n

T

(q

1

, . . . , q

n

) = H

(18)

równanie to daje 12 nieliniowych równań z n niewiadomymi

t

ij

(q

1

, . . . , q

n

) = h

ij

;

i

= 1, 2, 3; j = 1, 2, 3, 4

gdzie t

ij

oraz h

ij

12 nietrywialnych elementów macierzy

0

n

T

i H.

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

29

Istnienie rozwiązania OZK:

• Dla robota o 6 stopniach swobody mamy 6 niewiadomych oraz 12 równań. Jednak tylko 3 równania

z 9 wynikających z porównania elementów macierzy orientacji jest niezależnych, mamy zatem układ
6 równań z 6 niewiadomymi.

• Równania te są nieliniowe i przestępne.

• Brak jest ogólnych metod rozwiązywania układów równań nieliniowych.

• Może istnieć wiele rozwiązań dla danego położenia i orientacji (rozwiązania wielokrotne). W

szczególnych przypadkach (w punktach osobliwych) może być nieskończenie wiele rozwiązań.

• Liczba rozwiązań zależy od liczby przegubów, wartości parametrów D-H i dopuszczalnych zakresów

ruchu w przegubach.

• Na ogół im więcej niezerowych parametrów D-H tym więcej rozwiązań. Przykładowo dla robota 6R

zależność liczby rozwiązań od parametru a

i

jest następująca:

Wartość a

i

Liczba rozwiązań

a

1

= a

3

= a

5

= 0

¬ 4

a

3

= a

5

= 0

¬ 8

v a

3

= 0

¬ 16

∀ a

i

6= 0

¬ 16

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

30

Strategie rozwiązywania OZK:

• rozwiązania w postaci jawnej

• rozwiązania numeryczne (iteracyjne)

Wyróżnia się dwa podstawowe podejścia do rozwiązania OZK w postaci jawnej:

• metody algebraiczne

• metody geometryczne

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

31

Przykład 3: Rozwiązać OZK dla płaskiego manipulatora trójczłonowego pokazanego na rys.

4

.

Rozwiązanie algebraiczne.
Zadaną pozycję końcówki manipulatora można zapisać w postaci macierzy

H

=










c

ϕ

−s

ϕ

0 x

s

ϕ

c

ϕ

0 y

0

0

1 0

0

0

0 1










(19)

Porównując prawe strony równań (

16

) i (

19

) otrzymujemy układ 4 równań z 3 niewiadomymi θ

1

, θ

2

, θ

3

:

c

ϕ

= c

123

(20)

s

ϕ

= s

123

(21)

x

= L

1

c

1

+ L

2

c

12

(22)

y

= L

1

s

1

+ L

2

s

12

(23)

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

32

Podnosimy do kwadratu równania (

22

) i (

23

) i dodajemy stronami

x

2

+ y

2

= L

2

1

+ L

2

2

+ 2L

1

L

2

c

2

(24)

stąd

c

2

=

x

2

+ y

2

− L

2

1

− L

2

2

2L

1

L

2

(25)

Prawa strona równania (

25

) musi zawierać się w przedziale [−1, 1], aby punkt docelowy należał do

przestrzeni roboczej, wtedy

s

2

= ±

q

1 − c

2

2

(26)

Kąt θ

2

obliczamy jako

θ

2

= atan2(s

2

,

c

2

)

(27)

Mając obliczone θ

2

można rozwiązać (

22

) i (

23

) względem θ

1

:

x

= m

1

c

1

− m

2

s

1

(28)

y

= m

1

s

1

+ m

2

c

1

(29)

gdzie

m

1

= L

1

+ L

2

c

2

m

2

= L

2

s

2

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

33

Dokonujemy zamiany zmiennych

r

= +

q

m

2

1

+ m

2

2

(30)

γ

= atan2(m

2

,

m

1

)

(31)

to m

1

= r cos γ, m

2

= r sin γ. Równania (

28

) i (

29

) można zapisać jako

x

r

= cos γ cos θ

1

− sin γ sin θ

1

(32)

y

r

= cos γ sin θ

1

+ sin γ cos θ

1

(33)

lub

cos(γ + θ

1

) =

x

r

(34)

sin(γ + θ

1

) =

y
r

(35)

stąd

γ

+ θ

1

= atan2(

y
r

,

x

r

) = atan2(y, x)

(36)

czyli

θ

1

= atan2(y, x) − atan2(m

2

,

m

1

)

(37)

Jeśli x = y = 0 to (

37

) staje się nieokreślone i θ

1

może przyjąć dowolną wartość. Ostatecznie z (

20

) i

(

21

) można znaleźć

θ

1

+ θ

2

+ θ

3

= atan2(s

ϕ

,

c

ϕ

) = ϕ

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

34

stąd

θ

3

= ϕ − θ

1

− θ

2

(38)

Rozwiązanie geometryczne:

y

x

y

0

x

0

O

1

O

3

{0}

a

q

2

b

y

L

1

L

2

Rys. 6: Rozwiązanie geometryczne OZK dla płaskiego manipulatora 3R

Kąt α = π − (−θ

2

) = π + θ

2

. Z twierdzenia kosinusów obliczamy θ

2

x

2

+ y

2

= L

2

1

+ L

2

2

− 2L

1

L

2

cos(π + θ

2

)

ponieważ cos(π + θ

2

) = − cos θ

2

, mamy

cos θ

2

= c

2

=

x

2

+ y

2

− L

2

1

− L

2

2

2L

1

L

2

Wstęp do Robotyki – c

W. Szynkiewicz, 2006

35

s

2

= ±

p

1 − c

2

stąd θ

2

θ

2

= atan2(s

2

,

c

2

)

Drugie rozwiązanie jest dla θ

′

2

= −θ

2

.

Aby wyznaczyć θ

1

należy wyznaczyć kąty β i ψ

β

= atan2(y, x)

Stosując powtórnie twierdzenie kosinusów obliczamy

cos ψ =

x

2

+ y

2

+ L

2

1

− L

2

2

2L

1

px

2

+ y

2

gdzie arccos() musi być takie aby dla 0 6 ψ 6 π zachować relacje geometryczne. Kąt θ

1

θ

1

=

(

β

+ ψ gdy θ

2

<

0

β

− ψ gdy θ

2

>

0

Kąt θ

3

obliczamy

θ

1

+ θ

2

+ θ

3

= ϕ ⇒ θ

3

= ϕ − θ

1

− θ

2