WOJSKOWA AKADEMIA TECHNICZNA
INSTYTUT SYSTEMÓW MECHATRONICZNYCH
ZAKŁAD SYSTEMÓW STEROWANIA
INSTRUKCJA
DO PRZEPROWADZENIA WICZENIA LABORATORYJNEGO
NA TEMAT:
ANALIZA KINEMATYCZNA MANIPULATORÓW
– ZADANIE PROSTE KINEMATYKI
Mjr dr in . Wojciech KACZMAREK
Dr in . Jarosław PANASIUK
WARSZAWA 2008
ANALIZA KINEMATYCZNA MANIPULATORÓW – ZADANIE PROSTE KINEMATYKI
3
I. CZ
TEORETYCZNA
1. C
EL WICZENIA
Celem
wiczenia jest zapoznanie studentów z kinematyk manipulatorów robotów
przemysłowych oraz problemami powstaj cymi podczas analizy kinematyki w trybie symulacji
komputerowej. Studenci podczas wiczenia zapoznaj si ze rodowiskiem MATLAB oraz wykorzystaj
jego podstawowe funkcje do analizy kinematycznej manipulatora.
2. K
INEMATYKA MANIPULATORÓW
Kinematyka to nauka zajmuj ca si opisem ruchu bez uwzgl dnienia sił wywołuj cych ten
ruch. W jej ramach bada si zmiany poło enia, pr dko ci i przyspiesze członów manipulatora.
W zagadnieniach zwi zanych z kinematyk manipulatorów robotów mo na wyró ni dwa typy zada :
•
zadanie bezpo rednie (proste) kinematyki, które polega na wyznaczeniu pozycji i orientacji
efektora manipulatora wzgl dem układu podstawy przy znanych współrz dnych
konfiguracyjnych (odwzorowanie opisu poło enia manipulatora z przestrzeni współrz dnych
konfiguracyjnych do przestrzeni współrz dnych kartezja skich);
•
zadanie odwrotne kinematyki, które polega na wyznaczeniu wszystkich mo liwych zbiorów
współrz dnych konfiguracyjnych umo liwiaj cych osi gni cie zadanych pozycji i orientacji
manipulatora.
Przy rozwa aniach dotycz cych kinematyki obowi zuj nast puj ce zasady:
•
robota opisuje si za pomoc struktury kinematycznej (szkicu schematu kinematycznego), na
której zaznacza si człony oraz poł czenia;
•
oznaczenia osi współrz dnych, kierunków i zespołów ruchu, konieczne do jednoznaczno ci
opracowanego szkicu zapisane s w normie PN-93/M-55251;
•
robota opisuje si w trzech układach odniesienia:
o
globalnym (bazowym oznaczonym cyfr 0) – opis przemieszczenia robota wzgl dem
inercjalnego (nieruchomego) układu współrz dnych (najcz ciej wzgl dem stanowiska
roboczego);
o
regionalnym (oznaczonym cyframi k=1,2,3,,, rozpoczynaj c numeracj od członu
znajduj cego si najbli ej) – opis przemieszczenia manipulatora;
o
lokalnym (oznaczonym liter C) – opis przemieszczenia i orientacji chwytaka.
•
podstawowy układ osi jest prawoskr tnym układem kartezja skim (prostok tnym), gdzie osie x
i y tworz płaszczyzn poziom .
•
za dodatni przyjmuje si zwroty ruchów:
o
w ruchu liniowym na zewn trz mechanizmów (od pocz tku układów globalnego,
regionalnego, lokalnego);
o
w ruchu obrotowym w kierunku prawoskr tnym, zgodnie z przyj tym układem
współrz dnych.
•
numeracj ła cucha kinematycznego nale y rozpocz od podstawy (układu bazowego),
a zako czy przed efektorem (układem lokalnym).
ANALIZA KINEMATYCZNA MANIPULATORÓW – ZADANIE PROSTE KINEMATYKI
4
Przemieszczenie układu lokalnego wzgl dem bazowego opisuje wektor miejsca (
0
r
c
rys.2.1).
Łatwo zauwa y , e układ lokalny mo e by nie tylko przesuni ty wzgl dem układu bazowego, ale
równie obrócony (wzgl dem poszczególnych osi układu rys.2.1c). T drug operacj (obrót) mo na
opisa poprzez, tworzon za pomoc cosinusów kierunkowych macierz obrotu o wymiarach 3x3.
Rys.2.1. Widok wzajemnego poło enia układów współrz dnych:
a) układ i-tego ciała jest przemieszczony wzgl dem układu bazowego
b) układ i-tego ciała jest obrócony wzgl dem układu bazowego
c) układ i-tego ciała jest przemieszczony i obrócony wzgl dem układu bazowego
Oznaczenia:
•
prawy, dolny indeks oznacza nowy układ lub nowe poło enie po transformacji;
•
lewy górny indeks oznacza układ, wzgl dem którego dokonano transformacji;
•
0
i
– pocz tek układu współrz dnych ciała i;
•
C – zmienny punkt ciała;
•
z
y
x
e
e
e
0
0
0
,
,
– wektory jednostkowe układu inercjalnego;
•
z
i
y
i
x
i
e
e
e
)
(
0
)
(
0
)
(
0
,
,
– wektory jednostkowe układu ciała i przedstawione w układzie bazowym;
ANALIZA KINEMATYCZNA MANIPULATORÓW – ZADANIE PROSTE KINEMATYKI
5
•
C
r
0
– wektor miejsca w układzie bazowym;
•
C
i
r
– wektor miejsca w układzie lokalnym.
Dla przypadków szczególnych mo na zapisa nast puj ce zale no ci:
•
je li i-ty układ jest przesuni ty wzgl dem układu bazowego, a jego orientacja jest taka sama jak
układu bazowego (poszczególne osie układu s do siebie równoległe – rys.2.1a):
C
i
i
C
r
r
r
+
=
0
0
(1.1)
•
je li i-ty układ jest obrócony wzgl dem układu bazowego, a jego pocz tek układu pokrywa si z
pocz tkiem układu bazowego – rys.2.1b:
C
i
i
C
r
rot
r
⋅
=
0
0
(1.2)
Pełn transformacj współrz dnych (rys.2.1c) mo na przedstawi jako poł czenie przemieszczenia
(translacji) i obrotu (rotacji):
C
i
i
i
C
r
rot
r
r
⋅
+
=
0
0
0
(1.3)
Obroty elementarne
Zgodnie z ogólnie panuj cymi zasadami przyj to prawoskr tny układ współrz dnych (rys.2.2).
Rys.2.2. Obroty elementarne wzgl dem poszczególnych osi:
a) – osi x; b) – osi y; c) – osi z.
Dla poszczególnych obrotów elementarnych mo na zapisa nast puj ce zale no ci:
Obrót wokół osi x (rys.2.2a).
macierz obrotu wokół osi x mo na jest opisana wyra eniem:
−
=
φ
φ
φ
φ
φ
cos
sin
0
sin
cos
0
0
0
1
)
(
x
rot
(1.4)
ANALIZA KINEMATYCZNA MANIPULATORÓW – ZADANIE PROSTE KINEMATYKI
6
Obrót wokół osi y (rys.2.2b).
macierz obrotu wokół osi y mo na opisa wyra eniem:
( )
−
=
ψ
ψ
ψ
ψ
ψ
cos
0
sin
0
1
0
sin
0
cos
y
rot
(1.5)
Obrót wokół osi z (rys.2.2c).
macierz obrotu wokół osi z mo na opisa wyra eniem:
( )
−
=
1
0
0
0
cos
sin
0
sin
cos
θ
θ
θ
θ
θ
z
rot
(1.6)
Obroty zło one
Obroty zło one mog by tworzone z trzech kolejno po sobie wykonywanych obrotów
elementarnych. Nale y pami ta o tym, i mno enie macierzy nie jest przemienne, dlatego kolejno
wykonywania obrotów elementarnych jest wa na i wpływa na wynik ko cowy (posta macierzy obrotów
zło onych). Dowodem tego s macierze przedstawione poni ej jako wzory (1.7) i (1.8).
Dla uproszczenia zapisu u yto skrótów s
φ
=sin
φ
, c
φ
= cos
φ
itd.
( )
( )
( )
+
−
−
−
+
−
=
=
⋅
⋅
=
ψ
φ
θ
ψ
φ
θ
φ
θ
ψ
φ
θ
φ
ψ
φ
θ
ψ
φ
θ
φ
θ
ψ
φ
θ
φ
ψ
θ
ψ
θ
ψ
θ
ψ
φ
θ
ψ
φ
c
c
s
s
c
c
s
c
s
c
s
s
c
s
s
s
s
c
c
c
s
s
s
c
s
s
c
c
c
rot
rot
rot
rot
z
y
x
)
,
,
(
(1.7)
( )
( )
( )
−
+
−
+
+
+
−
=
=
⋅
⋅
=
ψ
φ
ψ
φ
ψ
θ
ψ
φ
θ
φ
θ
ψ
φ
θ
φ
ψ
θ
φ
ψ
θ
θ
φ
θ
ψ
φ
θ
φ
θ
ψ
φ
ψ
θ
φ
ψ
θ
s
c
c
s
s
s
s
c
c
s
s
s
s
c
c
c
s
c
s
c
s
s
c
s
s
s
c
c
s
rot
rot
rot
rot
x
y
z
)
,
,
(
(1.8)
Współrz dne i transformacje jednorodne.
Do analizy kinematycznej u ywa si tzw. współrz dnych jednorodnych. Punkt o współrz dnych
kartezja skich x,y,z opisuje si liczbami x
1
,x
2
,x
3
,x
4
, przy czym nie wszystkie z nich mog by
jednocze nie równe zero. Zale no mi dzy współrz dnymi prostok tnymi (x,y,z) i współrz dnymi
jednorodnymi mo na zapisa nast puj co:
4
3
4
2
4
1
x
x
z
x
x
y
x
x
x
=
=
=
(1.9)
Współrz dne jednorodne mo na okre li za pomoc wektora:
[
]
T
x
x
x
x
R
4
3
2
1
=
,
ANALIZA KINEMATYCZNA MANIPULATORÓW – ZADANIE PROSTE KINEMATYKI
7
Wprowadzenie współrz dnych jednorodnych umo liwiło utworzenie tzw., jednorodnej macierzy
transformacji o postaci:
4
4
1
1
1
000
000
x
i
i
i
i
R
r
rot
skali
czynnik
translacji
wektor
rotacji
macierz
T
∈
=
=
−
−
(1.10)
Czynnik skali (wzór 1.10) mo e opisywa np. odkształcenie obiektu w trójwymiarowej
przestrzeni jednak w robotyce, ze wzgl du na rozpatrywanie ciał sztywnych przyj to, i jest on równy
jeden.
W zwi zku z tym, i w kartezja skim układzie współrz dnych ciało mo e posiada sze stopni
swobody, zbiór wszystkich transformacji jednorodnych mo na przedstawi za pomoc sze ciu
elementarnych macierzy transformacji:
−
=
−
=
−
=
1
0
0
0
0
1
0
0
0
0
cos
sin
0
0
sin
cos
1
0
0
0
0
cos
0
sin
0
0
1
0
0
sin
0
cos
1
0
0
0
0
cos
sin
0
0
sin
cos
0
0
0
0
1
θ
θ
θ
θ
ψ
ψ
ψ
ψ
φ
φ
φ
φ
z
y
x
Rot
Rot
Rot
(1.11)
=
=
=
1
0
0
0
1
0
0
0
0
1
0
0
0
0
1
1
0
0
0
0
1
0
0
0
1
0
0
0
0
1
1
0
0
0
0
1
0
0
0
0
1
0
0
0
1
z
z
y
y
x
x
r
P
r
P
r
P
(1.12)
gdzie: r
x
, r
y
, r
z
– składowe wektora przemieszczenia.
Zadanie bezpo rednie (proste) kinematyki
Konfiguracja układu manipulatora jest realizowana poprzez zmienne konfiguracyjne
q=(q
1
,q
2
,..,q
n
) tzn. e ka dej warto ci zmiennej konfiguracyjnej odpowiada jedno poło enie chwytaka R
w układzie bazowym. Je li zało y si , i chwytak C jest zdefiniowany miejscem na ciele i układu
wielociałowego to mo na zapisa :
C
i
i
C
R
T
R
⋅
=
0
0
(1.13)
Zadanie bezpo rednie kinematyki mo na podzieli na etapy:
1. Usytuowanie manipulatora w poło eniu pocz tkowym i wprowadzenie układu bazowego.
2. Wprowadzenie układów regionalnych dla wszystkich członów manipulatora.
3. Wprowadzenie współrz dnych konfiguracyjnych.
4. Wyznaczenie wzajemnych poło e poszczególnych członów za pomoc jednorodnych macierzy
transformacji
n
i
T
i
i
,...
2
,
1
1
=
−
.
5. Wyznaczenie poło enia ko cówki manipulatora wzgl dem układu bazowego
i
i
i
T
T
T
T
1
2
1
1
0
0
...
−
⋅
⋅
⋅
=
.
6. Wyznaczenie zale no ci pomi dzy współrz dnymi bazowymi i współrz dnymi lokalnymi ko cówki
manipulatora
C
i
i
C
R
T
R
⋅
=
0
0
.
ANALIZA KINEMATYCZNA MANIPULATORÓW – ZADANIE PROSTE KINEMATYKI
8
II. CZ
PRAKTYCZNA
3. O
PIS STANOWISKA LABORATORYJNEGO
wiczenie laboratoryjne nale y wykona na komputerze PC w rodowisku MATLAB.
4. W
YKONANIE WICZENIA
wiczenie nale y wykona zgodnie z zaleceniami prowadz cego. Dla podanego typu
manipulatora (np. rys.4.1) nale y okre li transformacje układów współrz dnych w poszczególnych
zł czach i napisa program umo liwiaj cy analiz zadania bezpo redniego kinematyki.
Rys.4.1. Przykładowy widok manipulatora
UWAGA:
Podczas pracy w rodowisku MATLAB:
•
nie u ywaj specjalnych i polskich znaków nadaj c nazwy tworzonym plikom;
•
indeksy wpisuj zwykł czcionk .
Na zaj cia nale y przynie dyskietk 3,5”.
5. S
CHEMAT OBLICZE W RODOWISKU
MATLAB
i=1,2,3,……..
Pocz tek programu:
clear all
wyczy
wszystkie zmienne
close all
zamknij wszystkie okna
Zmienne wej ciowe
t
i
=0; i=1,2,3…
warto
pocz tkowa k ta
…….
tk
i
=50; i=1,2,3…
warto
ko cowa k ta
…….
t
i
=t
i
*pi/180; i=1,2,3…
przeliczenie warto ci pocz tkowej k ta na radiany
…….
tk
i
=tk
i
*pi/180; i=1,2,3… przeliczenie warto ci ko cowej k ta na radiany
…….
l
i
=15; i=1,2,3…
długo ci ramion manipulatora
…….
ANALIZA KINEMATYCZNA MANIPULATORÓW – ZADANIE PROSTE KINEMATYKI
9
Obszar wy wietlania
set(gca,'XlimMode','manual','YLimMode','manual','ZLimMode','manual'); umo liwia manualne definiowanie
przestrzeni roboczej
set(gca,'Xlim',[-50,50],'Ylim',[-50,50],'Zlim',[0,50]); definiowanie przestrzeni roboczej
i-ty człon manipulatora
Rx
i
=[1,0,0,0;0,cos(t
i
),-sin(t
i
),0;0,sin(t
i
),cos(t
i
),0;0,0,0,1]; zdefiniowanie macierzy obrotów wzgl dem osi x
Ry
i
=[ cos(t
i
),0, sin(t
i
),0;0,1,0,0;-sin(t
i
),0,cos(t
i
),0;0,0,0,1]; zdefiniowanie macierzy obrotów wzgl dem osi y
Rz
i
=[cos(t
i
),-sin(t
i
),0,0;sin(t
i
),cos(t
i
),0,0;0,0,1,0;0,0,0,1]; zdefiniowanie macierzy obrotów wzgl dem osi z
P
i
=[1,0,0, l
i
;0,1,0, l
i
;0,0,1,l
i
;0,0,0,1];
zdefiniowanie macierzy przesuni
T
i
=Rxyz
i
*P
i
;
wyznaczenie jednorodnej macierzy transformacji
x
i
=T
i
(1,4);
wyznaczenie warto ci współrz dnej x
y
i
=T
i
(2,4);
wyznaczenie warto ci współrz dnej y
z
i
=T
i
(3,4);
wyznaczenie warto ci współrz dnej z
i+1 człon manipulatora
Rx
i+1
=[1,0,0,0;0,cos(t
i+1
),-sin(t
i+1
),0;0,sin(t
i+1
),cos(t
i+1
),0;0,0,0,1]; zdefiniowanie macierzy obrotów
wzgl dem osi x
Ry
i+1
=[ cos(t
i+1
),0, sin(t
i+1
),0;0,1,0,0;-sin(t
i+1
),0,cos(t
i+1
),0;0,0,0,1]; zdefiniowanie macierzy obrotów
wzgl dem osi y
Rz
i+1
=[cos(t
i+1
),-sin(t
i+1
),0,0;sin(t
i+1
),cos(t
i+1
),0,0;0,0,1,0;0,0,0,1]; zdefiniowanie macierzy obrotów
wzgl dem osi z
P
i+1
=[1,0,0, l
i+1
;0,1,0, l
i+1
;0,0,1,l
i+1
;0,0,0,1] zdefiniowanie macierzy przesuni
T
i+1
=T
i
*Rxyz
i+1
*P
i+1
;
wyznaczenie jednorodnej macierzy transformacji
x
i+1
=T
i+1
(1,4);
wyznaczenie warto ci współrz dnej x
y
i+1
=T
i+1
(2,4);
wyznaczenie warto ci współrz dnej y
z
i+1
=T
i+1
(3,4);
wyznaczenie warto ci współrz dnej z
view(-27.5,30)
ustawienie k ta nachylenia wy wietlania układu współrz dnych
grid on
wy wietlenie siatki
ld
i
=line([0,x
i
],[0,y
i
],[0,z
i
],'LineWidth',6,'Color','red'); wykre
_
lenie i-tego członu manipulatora
ld
i+1
=line([x
i
,x
i+1
],[ y
i
,y
i+1
],[ z
i
,z
i+1
],'LineWidth',6,'Color','red'); wykre
_
lenie i+1 członu manipulatora
Ruch i-tego członu manipulatora
for t
i
=t
i
:0.05:tk
i
delete (ld
i
);
delete (ld
i+1
);
Tk1
i
=t
i
;
Rx
i
=[1,0,0,0;0,cos(Tk1
i
),-sin(Tk1
i
),0;0,sin(Tk1
i
),cos(Tk1
i
),0;0,0,0,1];
Ry
i
=[ cos(Tk1
i
),0, sin(Tk1
i
),0;0,1,0,0;-sin(Tk1
i
),0,cos(Tk1
i
),0;0,0,0,1];
Rz
i
=[cos(Tk1
i
),-sin(Tk1
i
),0,0;sin(Tk1
i
),cos(Tk1
i
),0,0;0,0,1,0;0,0,0,1];
P
i
=[1,0,0,0;0,1,0,0;0,0,1,l
i
;0,0,0,1];
T
i
=Rxyz
i
*P
i
;
x
i
=T
i
(1,4);
y
i
=T
i
(2,4);
z
i
=T
i
(3,4);
Rx
i+1
=[1,0,0,0;0,cos(t
i+1
),-sin(t
i+1
),0;0,sin(t
i+1
),cos(t
i+1
),0;0,0,0,1];
Ry
i+1
=[ cos(t
i+1
),0, sin(t
i+1
),0;0,1,0,0;-sin(t
i+1
),0,cos(t
i+1
),0;0,0,0,1];
Rz
i+1
=[cos(t
i+1
),-sin(t
i+1
),0,0;sin(t
i+1
),cos(t
i+1
),0,0;0,0,1,0;0,0,0,1];
P
i+1
=[1,0,0,0;0,1,0,0;0,0,1,l
i+1
;0,0,0,1];
T
i+1
=T
i
*Rxyz
i+1
*P
i+1
;
x
i+1
=T
i+1
(1,4);
y
i+1
=T
i+1
(2,4);
z
i+1
=T
i+1
(3,4);
grid on
ld
i
=line([0,x
i
],[0,y
i
],[0,z
i
],'LineWidth',6,'Color','red');
ANALIZA KINEMATYCZNA MANIPULATORÓW – ZADANIE PROSTE KINEMATYKI
10
ld
i+1
=line([x
i
,x
i+1
],[ y
i
,y
i+1
],[ z
i
,z
i+1
],'LineWidth',6,'Color','red');
pause(0.1);
end
Ruch i-tego członu manipulatora
for t
i+1
=t
i+1
:0.05:tk
i+1
delete (ld
i
);
delete (ld
i+1
);
Tk1
i+1
=t
i+1
;
Rx
i
=[1,0,0,0;0,cos(Tk1
i
),-sin(Tk1
i
),0;0,sin(Tk1
i
),cos(Tk1
i
),0;0,0,0,1];
Ry
i
=[ cos(Tk1
i
),0, sin(Tk1
i
),0;0,1,0,0;-sin(Tk1
i
),0,cos(Tk1
i
),0;0,0,0,1];
Rz
i
=[cos(Tk1
i
),-sin(Tk1
i
),0,0;sin(Tk1
i
),cos(Tk1
i
),0,0;0,0,1,0;0,0,0,1];
P
i
=[1,0,0,0;0,1,0,0;0,0,1,l
i
;0,0,0,1];
T
i
=Rxyz
i
*P
i
;
x
i
=T
i
(1,4);
y
i
=T
i
(2,4);
z
i
=T
i
(3,4);
Rx
i+1
=[1,0,0,0;0,cos(Tk1
i+1
),-sin(Tk1
i+1
),0;0,sin(Tk1
i+1
),cos(Tk1
i+1
),0;0,0,0,1];
Ry
i+1
=[ cos(Tk1
i+1
),0, sin(Tk1
i+1
),0;0,1,0,0;-sin(Tk1
i+1
),0,cos(Tk1
i+1
),0;0,0,0,1];
Rz
i+1
=[cos(Tk1
i+1
),-sin(Tk1
i+1
),0,0;sin(Tk1
i+1
),cos(Tk1
i+1
),0,0;0,0,1,0;0,0,0,1];
P
i+1
=[1,0,0,0;0,1,0,0;0,0,1,l
i+1
;0,0,0,1];
T
i+1
=T
i
*Rxyz
i+1
*P
i+1
;
x
i+1
=T
i+1
(1,4);
y
i+1
=T
i+1
(2,4);
z
i+1
=T
i+1
(3,4);
grid on
ld
i
=line([0,x
i
],[0,y
i
],[0,z
i
],'LineWidth',6,'Color','red');
ld
i+1
=line([x
i
,x
i+1
],[ y
i
,y
i+1
],[ z
i
,z
i+1
],'LineWidth',6,'Color','red');
pause(0.1);
end
UWAGA:
W programie nale y uwzgl dni tylko macierze obrotów i przesuni , które opisuj faktycznie tworzony
manipulator, tzn. je eli ruch pierwszego ramienia nast puje tylko wzgl dem osi x do programu nale y
wpisa tylko macierz rotacji wzgl dem tej osi.
W wyniku wiczenia powinien powsta program umo liwiaj cy przedstawienie ruchu manipulatora
(animacja) zgodnie z zaleceniami prowadz cego (m.in. zakresy ruchu poszczególnych osi).
6. W
YKONANIE SPRAWOZDANIA
W sprawozdaniu nale y zamie ci :
•
cel wiczenia;
•
szkic manipulatora;
•
przykłady zastosowa analizowanego typu robota;
•
wydruk programu z komentarzem;
•
widok manipulatora ze rodowiska MATLAB (skopiowany zrzut ekranu - Alt+PrtScr);
•
program w wersji elektronicznej.
7. P
YTANIA KONTROLNE
ANALIZA KINEMATYCZNA MANIPULATORÓW – ZADANIE PROSTE KINEMATYKI
11
1. Co to s współrz dne jednorodne podaj przykład?
2. Co to jest i jaka ma posta jednorodna macierz transformacji ?
3. Jakie maj postacie macierze obrotów elementarnych?
4. Co to jest czynnik skali (w mcierzy transformacj ijednorodnej) i do czego słu y?
5. Co to jest: zadanie bezposrednie/odwrotne kinematyki?
8. L
ITERATURA
1. Kaczmarek W., Panasiuk J..: „Podstawy robotyki” – opracowanie własne 2006.
2. Podr cznik u ytkownika pakietu Matlab.