Poruszane zagadnienia:

1.

Transmisyjne media bezprzewodowe.

2.

Zło

ż

ono

ść

zagadnienia kinematyki odwrotnej.

3.

Odsprz

ęż

enie kinematyczne.

4.

Kinematyka odwrotna pozycji – podej

ś

cie geometryczne.

5.

Kinematyka odwrotna orientacji,

Kinematyka odwrotna:

Rozwi

ą

zanie zadania kinematyki odwrotnej, sprowadza si

ę

do

wyznaczenia macierzy przekształcenia H (4

×

4), postaci

)

3

(

1

E

∈













=

0

d

R

H

Wyznaczenie tej macierzy polega na znalezieniu (jednego, b

ą

d

ź

wszystkich) rozwi

ą

za

ń

równania:

n

n

n

q

q

q

q

A

A

T

gdzie

H

T

⋅

⋅

=

=

...

)

,...,

(

)

,...,

(

1

6

1

0

6

1

0

Powy

ż

sze równanie daje 12 nieliniowych (nietrywialnych – gdy

ż

dolne wiersze

macierzy R i H to: (0, 0, 0, 1)) równa

ń

, które mo

ż

na zapisa

ć

4

3

2

1

3

2

1

)

,...,

(

,

6

1

,

,

,

; j

,

,

i

gdzie

h

q

q

j

i

i,j

=

=

=

T

Zło

ż

ono

ść

zagadnienia kinematyki odwrotnej:

Podczas rozwi

ą

zywania zadania kinematyki odwrotnej, jeste

ś

my zaintere-

sowani znalezieniem zamkni

ę

tej postaci rozwi

ą

zania równa

ń

, a nie rozwi

ą

-

zaniami numerycznymi, gdy

ż

:

w pewnych zastosowaniach, równania kinematyki odwrotnej musz

ą

by

ć

rozwi

ą

zywane bardzo szybko;

maj

ą

c rozwi

ą

zanie w postaci zamkni

ę

tej, mo

ż

na przygotowa

ć

zasady

wyboru konkretnych rozwi

ą

za

ń

spo

ś

ród wielu istniej

ą

cych.

Przykład. Manipulator stanfordzki

























=

⋅

⋅

⋅

⋅

⋅

=

1

0

0

0

33

32

31

23

22

21

13

12

11

6

5

4

3

2

1

6

0

z

y

x

d

r

r

r

d

r

r

r

d

r

r

r

A

A

A

A

A

A

T

W celu znalezienia zmiennych przegubowych:

ϕ

1

,

ϕ

2

,

ϕ

3

,

ϕ

4

,

ϕ

5

,

ϕ

6

, nale

ż

y

tutaj rozwi

ą

za

ć

skomplikowany układ równa

ń

trygonometrycznych.

Układ równa

ń

trygonometrycznych niezb

ę

dnych do wyznaczenia

zmiennych przegubowych robota stanfordzkiego:







































=

−

+

=

+

+

+

+

=

−

+

+

−

=

+

−

=

+

+

=

−

+

=

+

+

=

+

−

+

+

+

−

=

+

−

−

+

+

−

=

−

−

−

=

+

+

−

−

=

+

−

−

−

z

y

x

d

s

s

c

c

c

d

c

d

d

s

s

c

s

s

c

c

s

s

c

d

c

d

s

s

d

d

s

s

s

s

c

c

s

c

c

c

d

s

d

s

c

d

r

c

c

s

c

s

r

s

s

c

c

s

s

c

c

s

r

s

s

s

c

s

s

c

c

c

r

s

s

c

c

s

s

c

c

s

r

c

c

s

c

s

c

s

s

s

c

s

s

c

c

c

s

r

c

c

s

c

s

s

s

s

s

c

s

s

c

c

c

c

r

c

s

c

s

s

c

c

c

s

r

s

c

c

c

s

c

c

s

s

s

s

c

c

c

c

s

r

s

c

c

c

s

s

c

s

s

s

s

c

c

c

c

c

)

(

)

(

)

(

)

(

)

(

)

(

)

(

]

)

(

[

)

(

]

)

(

[

)

(

)

(

]

)

(

[

)

(

]

)

(

[

5

2

4

5

2

6

2

3

2

1

5

5

1

4

2

5

4

1

6

1

2

2

1

3

5

4

1

2

5

1

5

4

2

1

6

1

2

2

1

3

33

5

2

5

4

2

23

5

4

1

5

2

5

4

2

1

13

5

4

1

5

2

5

4

2

1

32

6

5

2

6

4

6

5

4

2

22

6

4

6

5

4

1

6

5

2

6

4

6

5

4

2

1

12

6

4

6

5

4

1

6

5

2

6

4

6

5

4

2

1

31

6

5

2

6

4

6

5

4

2

21

6

4

6

5

4

1

6

5

2

6

4

6

5

4

2

1

11

6

4

6

5

4

1

6

5

2

6

4

6

5

4

2

1

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

a

n

s

d

2

d

3

ϕ

6

ϕ

5

ϕ

4

ϕ

1

y

3

z

3

x

3

O

3

z

2

y

2

x

2

O

2

z

4

z

5

z

0

y

0

x

0

d

0

c

d

0

n

d

6

Rozwa

ż

my manipulator o sze

ś

ciu

stopniach swobody (np. manipula-
tor stanfordzki).

Odsprz

ęż

enie kinematyczne

a

n

s

d

2

d

3

ϕ

6

ϕ

5

ϕ

4

ϕ

1

y

3

z

3

x

3

O

3

z

2

y

2

x

2

O

2

z

4

z

5

z

0

y

0

x

0

d

0

c

d

0

n

d

d

oraz

R

R

:

równa

ń

dwóch

postaci

w

H

T

zale

ż

no

ść

wyrazimy

=

=

=

)

,...,

(

)

,...,

(

)

,...,

(

6

1

6
0

6

1

6
0

6

1

0

q

q

q

q

q

q

n

d

6

Rk

d

p

:

zatem

6

d

c

−

=

Analiza przypadku

Je

ś

li współrz

ę

dne wektora d oznaczy

ć

odpowiednio: d

x

, d

y

, d

z

,

a współrz

ę

dne wektora p

c

przez p

x

, p

y

, p

z

, to:





















−

−

−

=





















33

6

23

6

13

6

r

d

d

r

d

d

r

d

d

p

p

p

z

y

x

z

y

x

teraz, gdy mo

ż

liwe jest ju

ż

wyznaczenie 3 pierwszych zmiennych

przegubowych, korzystaj

ą

c z wyra

ż

enia

6
3

3
0

R

R

R

⋅

=

okre

ś

lamy orientacj

ę

ko

ń

cówki robota wzgl

ę

dem układu O

3

x

3

y

3

z

3

(na podstawie której wyznaczamy 3 pozostałe k

ą

ty przegubowe,

jako zbiór k

ą

tów Eulera odpowiadaj

ą

cych przekształceniu R

3

6

):

( )

( )

R

R

R

R

R

⋅

=

⋅

=

−

T

3
0

1

3
0

6
3

Algorytm realizacji powy

ż

szego zadania:

Krok 1. Znale

źć

zmienne przegubowe q

1

, q

2

, q

3

takie,

ż

e

ś

rodek ki

ś

ci

o wektorze p

c

jest zlokalizowany równaniem:

Krok 2. Wykorzystuj

ą

c zmienne przegubowe wyznaczone w kroku 1,

obliczy

ć

macierz przekształcenia R

0

3

.

Krok 3. Znale

źć

zestaw k

ą

tów Eulera odpowiadaj

ą

cych macierzy obrotu

R

3

6

na podstawie wzoru:

Rk

d

p

6

d

c

−

=

( )

( )

R

R

R

R

R

⋅

=

⋅

=

−

T

3
0

1

3
0

6
3

Kinematyka odwrotna pozycji – podej

ś

cie

geometryczne

Manipulator z łokciem – konfiguracja stawowa

p

c

p

c

= [p

x

, p

y

, p

z

]

T

Rzutowanie wektora p

c

na płaszczyzn

ę

x

0

y

0

z

2

P

y

P

x

S

z

r

x

0

y

0

x

1

z

1

z

0

ϕ

1

Zatem rzut

ś

rodka ki

ś

ci na płaszczyzn

ę

x

0

y

0

przedstawia si

ę

nast

ę

puj

ą

co:

ϕ

1

r

x

0

y

0

p

y

p

x

)

,

arctan(

1

y

x

p

p

=

ϕ

gdzie arctan(x,y) ozn. dwuargumentow

ą

funkcj

ę

arcus tangens zdefiniowan

ą

dla

wszystkich (x,y)

≠

0, wyznaczaj

ą

c

ą

jed-

noznacznie k

ą

t

ϕ

taki,

ż

e:

2

2

2

2

sin

,

cos

y

x

y

y

x

x

+

=

+

=

ϕ

ϕ

Na przykład:

4

3

)

1

,

1

arctan(

,

4

)

1

,

1

arctan(

π

π

=

−

−

=

−

natomiast

Jednocze

ś

nie nale

ż

y zauwa

ż

y

ć

,

ż

e drugim do-

puszczalnym rozwi

ą

zaniem dla k

ą

ta

ϕ

1

jest:

)

,

arctan(

1

y

x

p

p

+

=

π

ϕ

Pozycja osobliwa manipulatora (p

x

= p

y

= 0)

z

0

W pozycji nieosobliwej

ś

rodek

ki

ś

ci opisany wektorem p

c

le

ż

y

na osi z

0

, a wi

ę

c dla ka

ż

dej

warto

ś

ci k

ą

ta

ϕ

1

poło

ż

enie

ś

rodka jest stałe. Mamy wi

ę

c

do czynienia z niesko

ń

czon

ą

liczb

ą

rozwi

ą

za

ń

dla

ϕ

1

.

Manipulator z łokciem z przesuni

ę

tym barkiem

z

0

d

1

p

x

r

α

φ

ϕ

1

d

1

x

0

y

0

p

x

r

ϕ

1

d

1

x

0

y

0

Konfiguracja
lewego
ramienia

Konfiguracja
prawego
ramienia

p

y

p

y

Konfiguracja
lewego
ramienia:

p

x

r

α

φ

ϕ

1

d

1

x

0

y

0

(

)

(

)

2

1

2

2

1

2

1

2

1

1

1

,

arctan

,

arctan

)

,

arctan(

d

p

p

d

d

r

d

p

p

y

x

y

x

−

+

=

=

−

=

=

−

=

gdzie

α

ϕ

α

φ

ϕ

p

y

Konfiguracja
lewego
ramienia:

p

x

r

ϕ

1

d

1

x

0

y

0

p

y

(

)

2

1

2

2

1

1

,

arctan

)

,

arctan(

d

p

p

d

p

p

y

x

y

x

−

+

−

−

=

ϕ

Wyznaczenie k

ą

tów

ϕ

2

i

ϕ

3

manipulatora:

x

0

y

0

s

r

ϕ

2

ϕ

3

a

2

a

3

)

1

,

arctan(

2

3

M

M

−

±

=

ϕ

)

sin

,

cos

arctan(

)

,

arctan(

)

sin

,

cos

arctan(

)

,

arctan(

3

3

3

3

2

1

2

2

3

3

3

3

2

2

ϕ

ϕ

ϕ

ϕ

ϕ

a

a

a

d

p

p

p

a

a

a

s

r

z

y

x

+

−

−

+

=

=

+

−

=

Na mocy wzorów wyprowa-
dzonych na wykładzie pierw-
szym,

dla

dwuczłonowego

mechanizmu płaskiego, otrzy-
mujemy:

oraz

Manipulator z łokciem z przesuni

ę

ciem – robot PUMA

Rami

ę

z lewej strony u góry

Rami

ę

z prawej strony u góry

Rami

ę

z lewej strony u dołu

Rami

ę

z prawej strony u dołu

Konfiguracja sferyczna – manipulator sferyczny

x

0

z

0

ϕ

1

y

0

ϕ

2

r

s

d

1

d

3

p

x

p

z

p

y

a

2

Analiza manipulatora sferycznego

)

,

arctan(

)

,

arctan(

1

1

y

x

y

x

p

p

p

p

+

=

=

π

ϕ

ϕ

)

,

arctan(

)

,

arctan(

2

2

s

r

s

r

−

=

=

π

ϕ

ϕ

2

2

2

y

x

p

p

r

+

=

1

d

p

s

z

−

=

x

0

z

0

ϕ

1

y

0

ϕ

2

r

s

d

1

d

3

p

x

p

z

p

y

a

2

2

2

2

2

3

)

(

s

r

a

d

+

=

+

2

2

1

2

2

2

2

2

3

)

(

a

d

p

p

p

a

s

r

d

z

y

x

−

−

+

+

=

−

+

=

Kinematyka odwrotna orientacji – znajdowanie

k

ą

tów Eulera odpowiadaj

ą

cych macierzy obrotu R

W celu wyznaczenia k

ą

tów Eulera

αααα

,

ββββ

,

γγγγ

(w przypadku ki

ś

ci

sferycznej), nale

ż

y rozwi

ą

za

ć

równanie macierzowe postaci:





















=





















−

+

−

+

−

−

−

33

32

31

23

22

21

13

12

11

u

u

u

u

u

u

u

u

u

c

s

s

c

s

s

s

c

c

s

c

s

s

c

c

c

s

s

c

c

s

s

c

c

s

s

c

c

c

β

γ

β

γ

β

β

α

γ

α

γ

β

α

γ

α

γ

β

α

β

α

γ

α

γ

β

α

γ

α

γ

β

α

)

1

,

arctan(

lub

)

1

,

arctan(

1

1

0

.

2

)

0

0

(

0

0

.

1

2

33

33

2

33

33

2

33

33

33

23

13

23

13

23

13

u

u

u

u

u

s

u

c

u

u

u

u

u

s

u

u

−

−

=

−

=







−

±

=

=

±

=

⇒

=

∧

=

∨

=

∧

≠

⇒

≠

∧

β

β

β

β

β

zatem

a

:

i

Je

ś

li wybierzemy pierwsz

ą

warto

ść

k

ą

ta

ββββ

, wówczas s

ββββ

> 0 oraz:







−

=

=

)

,

arctan(

)

,

arctan(

32

31

23

13

u

u

u

u

γ

α

Je

ś

li wybierzemy za

ś

drug

ą

warto

ść

k

ą

ta

ββββ

, wówczas s

ββββ

< 0 oraz:







−

=

−

−

=

)

,

arctan(

)

,

arctan(

32

31

23

13

u

u

u

u

γ

α

Je

ś

li natomiast u

13

= u

23

= 0, to fakt

ż

e macierz U jest ortogonalna impliku-

je równo

ś

ci u

33

=

±±±±

1 i u

31

= u

32

= 0, co oznacza,

ż

e macierz U jest postaci:





















±

=

1

0

0

0

0

22

21

12

11

u

u

u

u

U

Je

ś

li u

33

= 1, wtedy c

ββββ

= 1 i s

ββββ

= 0, czyli

ββββ

= 0, a zatem równanie

macierzowe, które nale

ż

y rozwi

ą

za

ć

jest postaci:





















=





















+

+

+

−

+

=





















+

−

+

−

−

−

1

0

0

0

0

1

0

0

0

)

(

)

(

0

)

(

)

(

1

0

0

0

0

22

21

12

11

u

u

u

u

c

s

s

c

c

c

s

s

s

c

c

s

c

s

s

c

s

s

c

c

γ

α

γ

α

γ

α

γ

α

γ

α

γ

α

γ

α

γ

α

γ

α

γ

α

γ

α

γ

α

Sum

ę

k

ą

tów

αααα

i

γγγγ

mo

ż

na wyznaczy

ć

z równania:

)

,

arctan(

)

,

arctan(

12

11

21

11

u

u

u

u

−

=

=

+

γ

α

Je

ś

li u

33

= -1, wtedy c

ββββ

= -1 i s

ββββ

= 0, czyli

ββββ

=

ππππ

, a zatem równanie

macierzowe, które nale

ż

y rozwi

ą

za

ć

jest postaci:





















−

=





















−

−

−

−

−

−

−

1

0

0

0

0

1

0

0

0

)

(

)

(

0

)

(

)

(

22

21

12

11

u

u

u

u

c

s

s

c

γ

α

γ

α

γ

α

γ

α

Ró

ż

nic

ę

k

ą

tów

αααα

i

γγγγ

mo

ż

na wyznaczy

ć

z równania:

)

,

arctan(

)

,

arctan(

22

21

21

11

u

u

u

u

−

−

=

−

−

=

−

γ

α

Przykład. Analiza zadania kinematyki odwrotnej – 3

ostatnie zmienne manipulatora z łokciem





















+

−

+

−

+

−

+

−

+

−

+

=

0

)

(

)

(

)

(

)

(

)

(

)

(

3

2

3

2

1

3

2

1

3

2

1

1

3

2

1

3

2

1

3
0

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

c

s

c

s

s

c

s

s

s

c

c

c

R





















−

−

−

−

+

−

+

=

5

6

5

6

5

5

4

6

4

6

5

4

6

4

6

5

4

5

4

6

4

6

5

4

6

4

6

5

4

6
3

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

c

s

s

c

s

s

s

c

c

s

c

s

c

c

c

c

s

s

c

c

s

c

c

c

s

s

c

c

c

R

Równanie, które nale

ż

y rozwi

ą

za

ć

dla 3 ostatnich zmiennych

manipulatora jest postaci:

( )

R

R

gdzie

U,

R

R

R

6
0

3
0

6
3

=

=

=

T

Stosuj

ą

c rozwi

ą

zanie uzyskane w przypadku k

ą

tów Eulera, otrzymujemy:

23

1

13

1

33

33

3

2

23

3

2

1

13

3

2

1

5

4

23

33

3

2

23

3

2

1

13

3

2

1

5

4

13

)

(

)

(

)

(

)

(

)

(

)

(

r

c

r

s

u

r

c

r

s

s

r

s

c

s

s

u

r

s

r

c

s

r

c

c

s

c

u

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

−

=

+

−

+

−

+

−

=

=

+

−

+

+

+

=

=

Je

ś

li u

13

i u

23

nie s

ą

jednocze

ś

nie równe zeru, wówczas ze wzorów:

)

1

,

arctan(

lub

)

1

,

arctan(

2

33

33

2

33

33

u

u

u

u

−

−

=

−

=

β

β

otrzymujemy:

(

)

2

23

1

13

1

23

1

13

1

5

)

(

1

,

arctan

r

c

r

s

r

c

r

s

ϕ

ϕ

ϕ

ϕ

ϕ

−

−

±

−

=

Je

ś

li w powy

ż

szym równaniu zostanie wybrana dodatnia warto

ść

pierwiastka, wówczas k

ą

ty

ϕϕϕϕ

4

i

ϕϕϕϕ

6

obliczy

ć

mo

ż

na ze wzorów:







−

=

=

)

,

arctan(

)

,

arctan(

32

31

23

13

u

u

u

u

γ

α

zatem:











+

−

=

+

−

+

−

+

−

+

−

+

+

+

=

)

,

arctan(

)

)

(

)

(

)

(

,

)

(

)

(

)

(

arctan(

22

1

12

1

21

1

11

1

6

33

3

2

23

3

2

1

13

3

2

1

33

3

2

23

3

2

1

13

3

2

1

4

r

c

r

s

r

c

r

s

r

c

r

s

s

r

s

c

r

s

r

c

s

r

c

c

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

Podsumowanie rozwa

ż

a

ń

dla manipulatora z łokciem

(manipulatora stawowego)





















−

−

−

=





















33

6

23

6

13

6

r

d

d

r

d

d

r

d

d

p

p

p

z

y

x

z

y

x

Je

ś

li dysponujemy nast

ę

puj

ą

cymi danymi:





















=

z

y

x

d

d

d

d

oraz:





















=

33

32

31

23

22

21

13

12

11

r

r

r

r

r

r

r

r

r

R

wówczas zmienne przegubowe w notacji (D-H) s

ą

okre

ś

lone

nast

ę

puj

ą

cymi wzorami:

(

)



































+

−

=

−

−

±

−

=

+

−

+

−

+

−

+

−

+

+

+

=

−

−

−

+

+

=

−

±

=

+

−

−

+

=

=

)

,

arctan(

)

(

1

,

arctan

)

)

(

)

(

)

(

,

)

(

)

(

)

(

arctan(

2

)

(

),

1

,

arctan(

)

,

arctan(

)

,

arctan(

)

,

arctan(

22

1

12

1

21

1

11

1

6

2

23

1

13

1

23

1

13

1

5

33

3

2

23

3

2

1

13

3

2

1

33

3

2

23

3

2

1

13

3

2

1

4

3

2

2

3

3

2

2

1

2

2

2

3

3

3

3

3

2

1

2

2

2

1

r

c

r

s

r

c

r

s

r

c

r

s

r

c

r

s

r

c

r

s

s

r

s

c

r

s

r

c

s

r

c

c

a

a

a

a

d

p

p

p

M

gdzie

M

M

s

a

c

a

a

d

p

p

p

p

p

z

y

x

z

y

x

y

x

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

Przykład. Manipulator SCARA

Poniewa

ż

kinematyka prosta tego manipulatora okre

ś

lona jest przez

macierz T

0

4

(4 stopnie swobody), to kinematyka odwrotna sprowadza si

ę

do rozwi

ą

zania nast

ę

puj

ą

cego równania macierzowego:

























−

−

−

+

+

+

−

+

−

+

−

+

+

+

+

+

+

−

+

+

+

=













1

0

0

0

1

0

0

)

(

0

)

(

)

(

)

(

)

(

)

(

0

)

(

)

(

)

(

)

(

1

4

3

2

1

2

1

1

4

2

1

4

2

1

4

2

1

4

2

1

2

1

2

1

1

4

2

1

4

2

1

4

2

1

4

2

1

d

d

s

a

s

a

c

c

s

s

s

c

c

s

c

a

c

a

c

s

s

c

s

s

c

c

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

ϕ

0

d

R

Rozwi

ą

zanie powy

ż

szego równania jest mo

ż

liwe wtedy i tylko wtedy gdy

macierz R jest postaci:

)

,

arctan(

1

0

0

0

0

11

12

4

2

1

r

r

c

s

s

c

=

−

+

=





















−

−

=

ϕ

ϕ

ϕ

α

α

α

α

α

gdzie

,

R

Rzutowanie ramion manipulatora na płaszczyzn

ę

x

0

y

0

x

0

y

0

z

0

p

x

p

y

p

z

Wyznaczanie zmiennych przegubowych manipulatora

(na podstawie rysunku)

2

1

2

2

2

1

2

2

2

2

2

2

),

,

1

arctan(

a

a

a

a

d

d

r

r

r

y

x

−

−

+

=

−

±

=

gdzie

ϕ

)

,

arctan(

)

,

arctan(

2

2

2

2

1

1

ϕ

ϕ

ϕ

s

a

c

a

a

d

d

y

x

+

−

=

K

ą

t

ϕϕϕϕ

4

wyznaczamy z zale

ż

no

ś

ci:

)

,

arctan(

11

12

4

2

1

r

r

=

−

+

=

ϕ

ϕ

ϕ

α

Zatem:

)

,

arctan(

11

12

2

1

2

1

4

r

r

−

+

=

−

+

=

ϕ

ϕ

α

ϕ

ϕ

ϕ

4

3

d

d

d

z

+

=

oraz: