ANALIZA KINEMATYCZNA

MANIPULATORÓW

ROBOTÓW METODĄ

MACIERZOWĄ

Jeśli dane są:

- współrzędne

w

xi

, w

yi

, w

zi

wektora w związane z ogniwem i

- współrzędne

p

x

, p

y

, p

z

początku układu i związane z ogniwem j

oraz

kosinusy kierunkowe osi układu i względem osi układu j





j

i

x

x

x

l

,

cos







j

i

y

y

x

l

,

cos







j

i

z

z

x

l

,

cos







j

i

x

x

y

m

,

cos







j

i

y

y

y

m

,

cos







j

i

z

z

y

m

,

cos







j

i

x

x

z

n

,

cos







j

i

y

y

z

n

,

cos







j

i

z

z

z

n

,

cos



to zależność pomiędzy współrzędnymi w układzie i
oraz j można zapisać jako

x

zi

x

yi

x

xi

x

xj

p

w

n

w

m

w

l

w









y

zi

y

yi

y

xi

y

yj

p

w

n

w

m

w

l

w









z

zi

z

yi

z

xi

z

zj

p

w

n

w

m

w

l

w









lub w formie symbolicznej

i

ij

j

w

w

T



gdzie:

T

T 

ij

w



- macierz przekształcenia współrzędnych

wektora z układu i do układu j

Wyznacznik macierzy

T

T 

ij



ij

T

{

1, gdy obydwa układy są prawoskrętne

-1, gdy jeden jest prawo- a drugi lewoskrętny

Macierz

T

ij

przekształcenia

złożonego

z

przesunięcia i obrotu można przedstawić w
postaci iloczynu macierzy:

0

T

ij

- obrotu
(rotacji)

p

ij

T

- przesunięcia (translacji)

czyli

p

ij

ij

ij

T

T

T

0





gdzie:



























1

0

0

0

0

0

0

T

0

z

z

z

y

y

y

x

x

x

ij

n

m

l

n

m

l

n

m

l



























1

0

0

0

1

0

0

0

1

0

0

0

1

z

y

x

p

ij

p

p

p

T

Zastosowanie do opisu przekształceń w kinematyce
i dynamice manipulatorów robotów macierzy
4x4 jest bardzo wygodne ponieważ umożliwia w
zwartej formie zapisać zarówno obrót jak i
przesunięcie oraz ułatwia mnożenie odpowiednich
macierzy przy użyciu komputerów osobistych bez
konieczności sprawdzania osobliwości.

0

T

ij

- obrotu (rotacji)

tylko trzy są niezależne, natomiast pozostałe
sześć muszą spełniać równania

Z dziewięciu zmiennych wyrazów macierzy

1

2

2

2







z

y

x

l

l

l

1

2

2

2







z

y

x

m

m

m

1

2

2

2







z

y

x

n

n

n

0







z

z

y

y

x

x

m

l

m

l

m

l

x

y

z

z

y

n

m

l

m

l





y

z

x

x

z

n

m

l

m

l





przy czy kwadraty kosinusów kierunkowych (trzy
pierwsze równania) są równe odpowiednim
kwadratom współrzędnych wersorów osi, których
długość jest równa 1; pozostałe trzy równania
wynikają z warunków prostopadłości wersorów osi
układu współrzędnych.

współrzędnych

przypadku

przekształceń

odwrotnych to znaczy przy przejściu z układu j do
układu i stosuje się macierze odwrotne, czyli

1

T

T





ij

ij

przy czym

E

T

T





ji

ij

gdzie: E jest macierzą jednostkową, czyli





























1

0

0

0

0

1

0

0

0

0

1

0

0

0

0

1

T

T

ji

ij

W celu uproszczenia analizy przestrzennego
łańcucha

kinematycznego

wprowadza

się

specjalne usytuowanie układów współrzędnych
poszczególnych

członów

tak,

aby

liczba

parametrów

wchodzących

do

macierzy

przekształceń była minimalna, a postać tej
macierzy była jednakowa tak w przypadku pary
obrotowej, jak i pary przesuwnej.
W dalszym ciągu przedstawiono najczęściej
stosowany sposób usytuowania wzajemnego
układów współrzędnych członów połączonych
parami obrotowymi i przesuwnymi, który jest
znany jako zapis

Hartenberga i Denavita

Na

rysunku

przedstawiono

dwa

układy

współrzędnych związanych z członami i – 1 oraz i,
(rys. ***)

Figure 4-8 The variables in a link using the
notation of Paul [9]. The rules used to define the
notation are: (1) Axis z

n-1

defines the position of the

axis of rotation for joint J

n

, z

n

, for joint J

n+1

, and so

forth. (2) Axis x

n-1

is selected to be an extension for

the common perpendicular line of length a

n-1

between consecutive joints z

n-2

and z

n-1

. (3) The

axis y

n-1

– is selected to provide a right-hand

coordinate system with the other axes. (4) Axis x

n

is an extension of the common perpendicular line
of Iength a

n

.

Rys 4-8. Zapis zmiennych z użyciem notacji Paul’a
[7]. Według zasad: (1) Oś z

n-1

opisuje położenie osi

rotacji dla ogniwa J

n

, zaś oś z

n

dla ogniwa J

n+1

itd.

(2) Oś x

n-1

jest przedłużeniem linii znajdującej się

pomiędzy osią z

n-2

i z

n-1

i prostopadłą do nich o

długości a

n-1

. (3) Oś y

n-1

zapewnia prawoskrętny

układ współrzędnych. (4) Oś x

n

jest przedłużeniem

linii prostopadłej do osi z

n-1

o długości a

n

.

Usytuowanie układów współrzędnych

Usytuowanie układów współrzędnych

leży na wspólnej prostopadłej do osi par obrotowych
członu i-1, oś

przy czym oś

x

i-1

z

i-1

leży na osi pary obrotowej łączącej człony

i-1 z i

oś

y

i-

1

nie pokazana na rysunku stanowi uzupełnienie
prawoskrętnego układu współrzędnych

i-
1

Układ współrzędnych jest związany z członem

i

w podobny sposób, to znaczy oś

x

i

leży na wspólnej prostopadłej do osi par obrotowych członu

i

leży na osi pary obrotowej łączącej człon

z

i

oś

i z członem i + 1

Zaletą takiego usytuowania układów współrzędnych
jest to, że tylko cztery parametry określają
względne położenie dwóch sąsiednich układów,
przy czym dwa z nich to znaczy

l

i

oraz

α

i

są zawsze stałe, jeden z pozostałych jest zmienny
w zależności od typu pary kinematycznej

- w przypadku pary obrotowej zmienny jest kąt

θ

i

- w przypadku pary przesuwnej zmienne jest przesunięcie

λ

i

Dwa sąsiednie układy współrzędnych

i oraz i-1

mogą być przekształcone jeden w drugi za pomocą

obrotu

dwóch przesunięć

i jeszcze jednego obrotu

w następującej kolejności

a) obrót wokół osi

z

i-1

o kąt θ

i

tak, aż oś x

i-1

stanie się równoległa do osi x

i

b) przesunięcie wzdłuż osi

z

i-1

o wielkość λ

i

tak, aby oś x

i-1

pokryła się z osią x

i

c) przesunięcie wzdłuż osi

x

i

o wielkość l

i

tak, aby początki obu układów pokryły się

d) obrót wokół osi

x

i

o kąt

α

i

tak aż wszystkie osie będą pokrywać się

Każdemu z tych elementarnych ruchów odpowiada macierz

T

i,i-1

Przekształcenia, którą tutaj oznaczono przez

A

i

przy czym





























1

0

0

0

0

1

0

0

0

0

cos

sin

0

0

sin

cos

A

i

i

i

i

a

i



































1

0

0

0

1

0

0

0

0

1

0

0

0

0

1

A

i

b
i





























1

0

0

0

0

1

0

0

0

0

1

0

0

0

1

A

i

c
i

l





























1

0

0

0

0

cos

sin

0

0

sin

cos

0

0

0

0

1

A

i

i

i

i

d

i









Macierz A

i

opisująca przekształcenia z układu

i do układu i-1

będzie równa iloczynowi wyżej wymienionych
macierzy ruchów elementarnych, czyli

d

i

c

i

b
i

a

i

i

A

A

A

A

A









Zatem, po wykonaniu mnożeń macierzy zaczynając
od prawej strony otrzymuje się





























1

0

0

0

cos

sin

0

sin

sin

cos

cos

cos

sin

cos

sin

sin

cos

sin

cos

A

i

i

i

i

i

i

i

i

i

i

i

i

i

i

i

i

i

i

l

l































gdzie:

l

i

, α

i

- odległość i kąt pomiędzy osiami par

obrotowych ogniwa i,

λ

i

, θ

i

, - odległość i kąt obrotu pomiędzy

ogniwami i-1 oraz i

Przypadku pary obrotowej kąt θ

i

jest zmienny, a

odległość λ

i

jest stała; w przypadku pary przesuwnej

zmienna jest długość λ

i

a stały kąt θ

i

Macierz przekształcenia odwrotnego, to znaczy
układu współrzędnych członu i-1 do układu członu i
otrzymuje

się

jako

rozwiązanie

równania

macierzowego

E

A

A

1





i

i

gdzie: E – macierz jednostkowa

stąd







































1

0

0

0

cos

cos

sin

cos

sin

sin

sin

sin

cos

cos

cos

sin

0

sin

cos

A

1

i

i

i

i

i

i

i

i

i

i

i

i

i

i

i

i

i

i

l

































W przypadku otwartego łańcucha kinematycznego
wprowadza się macierz położenia i orientacji
układu związanego z członem n względem układu
związanego z członem i jako iloczyn macierzy
kolejnych przekształceń

n

i

i

i

n

A

...

A

A

T

2

1

,













przy czym

1

1

,

A

T







i

i

i

W

przypadku

zamkniętego

łańcucha

kinematycznego, zbudowanego z n członów
wprowadza się równanie zamknięcia w postaci

E

A

...

A

A

2

1









n

Mnożąc z lewej strony powyższe równanie przez
macierze odwrotne otrzymuje się równanie
zamknięcia łańcucha kinematycznego w bardziej
wygodnej postaci

1

1

3

2

A

A

...

A

A











n

1

1

1

2

4

3

A

A

A

...

A

A













n

1

1

1

2

1

3

5

4

A

A

A

A

...

A

A















n

………………….……………………

W przypadku, gdy dany jest wektor

 

i

pi

r

opisujący

położenie

dowolnego

punktu

P

i

należącego do członu i w układzie współrzędnych
związanym z tym członem oraz dane są macierze
kolejnych przekształceń, wtedy z równań











































































1

1

0

0

0

1

zi

yi

xi

z

z

z

z

y

y

y

y

x

x

x

x

zj

yj

xj

w

w

w

p

n

m

l

p

n

m

l

p

n

m

l

w

w

w

oraz

n

i

i

i

n

A

...

A

A

T

2

1

,













można wyznaczyć wektor

 

0

pi

r

opisujący położenie punktu P

i

w układzie podstawy

 

 

i

pi

i

pi

r

r















A

...

A

A

2

1

0

przy czym mnożenia macierzy trzeba zaczynać od
prawej strony (!!!)

Kolejność obliczeń jest zatem następująca

 

 

i

pi

i

i

pi

r

r











A

1

 

 

1

1

2

A











i

pi

i

i

pi

r

r





 

 

2

2

3

A











i

pi

i

i

pi

r

r





………….………………

 

 

1

1

0

A

pi

pi

r

r









(wzory na r)

Wektory prędkości i przyspieszenia punktu P

i

otrzymuje się jako pierwszą i drugą pochodną
względem czasu wektora

 

0

pi

r

A zatem

 

 

0

0

d

d

pi

pi

pi

r

t

r

v











 

 

0

2

2

0

d

d

pi

pi

pi

r

t

r

a











Różniczkując kolejno dwie ostatnie zależności przy
założeniu, że

 

idem

r

i

pi





otrzymuje się algorytm wyznaczania prędkości

pi

v

jako

 

 

i

pi

i

i

pi

r

v













A

1

 

 

 

1

1

1

1

2

A

A



















i

pi

i

i

pi

i

i

pi

v

r

v









……………………………………………

 

 

 

2

2

2

1

1

A

A

pi

pi

i

pi

v

r

v



















 

 

 

1

1

1

1

0

A

A

pi

pi

pi

v

r

v

















(wzory na v)

oraz algorytm wyznaczania przyśpieszenia

pi

a

jako

 

 

i

pi

i

i

pi

r

a













A

1

 

 

 

 

1

1

1

1

1

1

2

A

2

A

A

























i

pi

i

i

pi

i

i

pi

i

i

pi

v

a

r

a













………………………………………………….

 

 

 

 

2

2

2

2

2

2

1

A

2

A

A

pi

pi

pi

pi

v

a

r

a























 

 

 

 

1

1

1

1

1

1

0

A

2

A

A

pi

pi

pi

pi

v

a

r

a























(wzory na a)

Pochodne względem czasu macierzy

A

j

oblicza się według następujących wzorów

j

j

j

A

Q

A 







j

j

j

j

A

Q

Q

A

2



 



(wzory na A)

przy czym w przypadku pary obrotowej

































0

0

0

0

0

0

0

0

0

0

0

0

0

0

Q

j

j

j









































0

0

0

0

0

0

0

0

0

0

0

0

0

0

Q

j

j

j













































0

0

0

0

0

0

0

0

0

0

0

0

0

0

Q

2

2

2

j

j

j









natomiast w przypadku pary przesuwnej



























0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

Q

j

j































0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

Q

j

j







0

Q

2



j

Podstawiając (wzory na A) do (wzorów na v) i
(wzorów na a) przy uwzględnieniu (wzorów na r)
otrzymuje się

 

 

1

1

Q









i

pi

i

i

pi

r

v





 

 

 

1

1

2

1

2

A

Q



















i

pi

i

i

pi

i

i

pi

v

r

v







…………………….……………………

 

 

 

2

2

1

2

1

A

Q

pi

pi

pi

v

r

v















 

 

 

1

1

0

1

0

A

Q

pi

pi

pi

v

r

v















(wzory na v)1

oraz

 





 

1

2

1

Q

Q











i

pi

i

i

i

pi

r

a







 





 

 

 

1

1

1

1

1

2

2

1

1

2

A

Q

2

A

Q

Q

































i

pi

i

i

i

pi

i

i

pi

i

i

i

pi

v

a

r

a











 





 

 

 

2

2

2

2

2

1

2
2

2

1

A

Q

2

A

Q

Q

pi

pi

pi

pi

v

a

r

a























 





 

 

 

1

1

1

1

1

0

2

1

1

0

A

Q

2

A

Q

Q

pi

pi

pi

pi

v

a

r

a























……………………………………..

(wzory na a)1

PRZYKŁAD

Gdy i = 2

wtedy (wzory na r) przyjmą postać

 

 

2

2

2

1

2

A

p

p

r

r









 

 

1

2

1

0

2

A

p

p

r

r









natomiast (wzory na v)1 są następujące

 

 

1

2

2

1

2

Q

p

p

r

v









 

 

 

1

2

1

0

2

1

0

2

A

Q

p

p

p

v

r

v















natomiast ( wzory na a)1 przyjmą formę

 





 

1

2

2
2

2

1

2

Q

Q

p

p

r

a













 





 

 

 

1

2

1

1

1

2

1

0

2

2

1

1

0

2

A

Q

2

A

Q

Q

p

p

p

p

v

a

r

a



























Wektory prędkości kątowej





i przyspieszenia kątowego





członów

można wyznaczyć z następujących równań:

1

,

1

2

1

32

2

1

21

1

1

A

...

A

A

...

...

A

A

A



















i

i

i

i





















(wzór na prędkość kątową członów)

21

1

21

1

1

1

2

A

A

Q































32

2

1

32

2

2

1

2

1

1

2

3

A

A

Q

A

A

A

Q





























A

………………………………………

1

,

1

2

1

1

,

1

1

2

1

1

3

2

2

1

1

i

2

1

1

1

A

...

A

A

A

Q

...

A

A

...

...

A

...

A

A

Q

A

...A

A

A

Q





























































i

i

i

i

i

i

i

i

i

i

















(wzór na przyśpieszenia kątowe członów)

gdzie:





T

i

i

i

i

0

0

0

0

0

0

1

,















































T

i

i

i

i

0

0

0

0

0

0

1

,









































