W raporcie przedstawione zostały metody obliczania kinematyki prostej i odwrotnej dla manipulatora typu SCARA. Dodatkowo na podstawie zamieszonych tutaj wzorów wykonany został skrypt w środowisku MATLAB, który umożliwia animację 3D pracy robota.

Kinematyka prosta

Schemat kinematyczny manipulatora z przyporządkowanymi układami współrzędnych zgodnie z algorytmem D-H.

Tabela D-H

Macierze jednorodne ⁰H_1, ¹H_2, ²H_3, ³H_4, ⁴H₅ kolejnych układów współrzędnych oraz macierz jednorodna ⁰H₅ opisująca położenie i orientację ostatniego układu względem układu bazowego.

Układ	θi	εi	ai	αi
^0H1	0	ε1	0	0
^1H2	θ2*	0	a2	0
^2H3	θ3*	0	a3	180^o
^3H4	0	ε4*	0	0
^4H5	θ5*	ε5	0	0

Przekształcenia jednorodne są w robotyce używane do obliczania zadań prostych i odwrotnych manipulatorów robotów.
Proste zadanie kinematyki polega na obliczeniu pozycji i orientacji członu roboczego względem układu odniesienia podstawy dla danego zbioru współrzędnych konfiguracyjnych. Dla złącz obrotowych podawane są wartości kątów między ogniwami, a dla złącz przesuwnych wartość wysunięcia ogniw.

H_i − 1ⁱ = Rot(Z,θ_i)Trans(0,0,ε_i)Trans(a_i,0,0)Rot(X,α_i) ∖ n

są parametrami D-H, i=1,2,…,n.

$$H = \ \begin{bmatrix} R & p \\ 0 & 1 \\ \end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} & p_{x} \\ r_{21} & r_{22} & r_{23} & p_{y} \\ r_{31} & r_{32} & r_{33} & p_{z} \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}\backslash n$$

Wymiary macierzy R i wektora d są ściśle związane z liczbą wymiarów przestrzeni, w której dokonywane są przekształcenia.

Podstawowe macierze obrotu danego elementu (punkt, bryła, układ) względem kolejnych osi X, Y i Z, mają postać:

$\text{Rot}\left( X,\theta \right) = \ \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & cos(\theta) & - \sin\left( \theta \right) & 0 \\ 0 & sin(\theta) & cos(\theta) & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}$ – obrót względem osi X o kąt θ.

$\text{Rot}\left( Y,\theta \right) = \ \begin{bmatrix} cos(\theta) & 0 & sin(\theta) & 0 \\ 0 & 1 & 0 & 0 \\ - sin(\theta) & 0 & cos(\theta) & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}$ – obrót względem osi Y o kąt θ.

$\text{Rot}\left( Z,\theta \right) = \ \begin{bmatrix} cos(\theta) & - sin(\theta) & 0 & 0 \\ sin(\theta) & cos(\theta) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}$ – obrót względem osi Z o kąt θ.

Macierz przesunięcia w poszczególnych osiach wygląda następująco:

$$\text{Trans}\left( p_{x},p_{y},p_{z} \right) = \begin{bmatrix} 1 & 0 & 0 & p_{x} \\ 0 & 1 & 0 & p_{y} \\ 0 & 0 & 1 & p_{z} \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}$$

Macierze jednorodne dla kolejnych układów:

$H_{0}^{1} = Trans\left( 0,0,\varepsilon_{1} \right) = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & \varepsilon_{1} \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}$ – macierz jednorodna z układu 0 do układu 1

$H_{1}^{2} = Rot(Z,\theta_{2})Trans\left( a_{2},0,0 \right) = \begin{bmatrix} cos(\theta_{2}) & - sin(\theta_{2}) & 0 & 0 \\ sin(\theta_{2}) & cos(\theta_{2}) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}\begin{bmatrix} 1 & 0 & 0 & a_{2} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} = \begin{bmatrix} cos(\theta_{2}) & - sin(\theta_{2}) & 0 & a_{2}cos(\theta_{2}) \\ sin(\theta_{2}) & cos(\theta_{2}) & 0 & a_{2}(sin(\theta_{2}) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}$ – macierz jednorodna z układu 1 do układu 2

$H_{2}^{3} = Rot\left( Z,\theta_{3} \right)\text{Trans}\left( a_{3},0,0 \right)\text{Rot}\left( X,180 \right) = \begin{bmatrix} \cos\left( \theta_{3} \right) & - \sin\left( \theta_{3} \right) & 0 & 0 \\ \sin\left( \theta_{3} \right) & \cos\left( \theta_{3} \right) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}\begin{bmatrix} 1 & 0 & 0 & a_{3} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & \cos\left( 180 \right) & - \sin\left( 180 \right) & 0 \\ 0 & \sin\left( 180 \right) & \cos\left( 180 \right) & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} = \begin{bmatrix} cos(\theta_{3}) & sin(\theta_{3}) & 0 & a_{3}cos(\theta_{3}) \\ sin(\theta_{3}) & - cos(\theta_{3}) & 0 & a_{3}(sin(\theta_{3}) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}$ – macierz jednorodna z układu 2 do układu 3

$H_{3}^{4} = Trans\left( 0,0,\varepsilon_{4} \right) = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & \varepsilon_{4} \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}$ – macierz jednorodna z układu 3 do układu 4

$H_{4}^{5} = Rot\left( Z,\theta_{5} \right)\text{Trans}\left( 0,0,\varepsilon_{5} \right) = \begin{bmatrix} \cos\left( \theta_{5} \right) & - \sin\left( \theta_{5} \right) & 0 & 0 \\ \sin\left( \theta_{5} \right) & \cos\left( \theta_{5} \right) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & \varepsilon_{5} \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} = \begin{bmatrix} cos(\theta_{5}) & - sin(\theta_{5}) & 0 & 0 \\ sin(\theta_{5}) & cos(\theta_{5}) & 0 & 0 \\ 0 & 0 & 1 & \varepsilon_{5} \\ 0 & 0 & 0 & 0 \\ \end{bmatrix}$ – macierz jednorodna z układu 4 do układu 5

H₀⁵ = H₀¹ * H₁² * H₂³ * H₃⁴ * H₄⁵

$H_{0}^{5} = \begin{bmatrix} cos(\theta_{2} + \theta_{3} - \theta_{5}) & sin(\theta_{2} + \theta_{3} - \theta_{5}) & 0 & a_{3}\cos\left( \theta_{2} + \theta_{3} \right) + a_{2}cos(\theta_{2}) \\ sin(\theta_{2} + \theta_{3} - \theta_{5}) & - cos(\theta_{2} + \theta_{3} - \theta_{5}) & 0 & a_{3}\sin\left( \theta_{2} + \theta_{4} \right) + a_{2}sin(\theta_{2}) \\ 0 & 0 & - 1 & - \varepsilon_{5} - \varepsilon_{4} + \varepsilon_{1} \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}$
– macierz jednorodna z układu bazowego do układu ostatniego

Kinematyka odwrotna

Odwrotne zadanie kinematyki polega na wyznaczeniu wszystkich możliwych współrzędnych konfiguracyjnych, które pozwolą osiągnąć manipulatorowi zadanej pozycji i orientacji członu roboczego. Ze względu na mnogość rozwiązań zadanie to jest na ogół trudniejsze od zadania kinematyki prostej.

Metody rozwiązywana odwrotnego zadania kinematyki:

• bezpośrednie podejście algebraiczne

• podejście geometryczne

• metoda jakobianu odwrotnego

Do rozwiązania zadania kinematyki odwrotnej wykorzystałem podejście geometryczne.

Rozwiązanie zadania kinematyki odwrotnej poprzez podejście geometryczne.

Na rysunku przedstawiono rzutowanie ramion manipulatora na płaszczyznę X₀ i Y₀.

Po etapie rzutowania można w łatwy sposób wyznaczyć wartości kątów θ₂ i θ₃.

W danym przypadku rozważam konfigurację „łokieć u dołu”.

Wykorzystują prawo cosinusów można wyznaczyć kąt θ₃.

(x²+y²) = a₂² + a₃² − 2a₂a₃cos(180−θ₃)

cos(180−θ₃) = −cos(θ₃)

$\cos\left( \theta_{3} \right) = \frac{x^{2} + y^{2} - a_{2}^{2} - a_{3}^{2}}{2a_{2}a_{3}} = D$

θ₃ = arccosD

takie rozwiązanie nie jest jednoznaczne, aby temu zapobiec wprowadzamy sinusa

$\sin{(\theta}_{3}) = \pm \sqrt{1 - D^{2}}$

$$\theta_{3} = arctan\left( \frac{\pm \sqrt{1 - D^{2}}}{D} \right)$$

Znak „+” lub „-” trzeba wybrać zgodnie z tym, jaka pozycja „łokcia” została wybrana wcześniej. Dalej zgodnie z tym co było na rysunku trzeba wyznaczyć θ₂ przy pomocy kątów α i β.

$\alpha = arctan\left( \frac{y}{x} \right)$

Stosując prawo sinusów można wyznaczyć wartość kąta β.

$\beta = arctan\left( \frac{a_{3}\sin\theta_{3}}{a_{2} + a_{3}\cos\theta_{3}} \right)$

θ₂ = α − β, czyli

$$\theta_{2} = arctan\left( \frac{y}{x} \right) - arctan\left( \frac{a_{3}\sin\theta_{3}}{a_{2} + a_{3}\cos\theta_{3}} \right)$$

Konieczne jest jeszcze wyznaczenie ε₄ oraz θ₅. Wartość ε₄ można przy pomocy wektora położenia pznajdującego się w macierzy H₀⁵.

$\begin{bmatrix} p_{0,5\ x} \\ p_{0,5\ y} \\ p_{0,5\ z} \\ \end{bmatrix} = \begin{bmatrix} a_{3}\cos\left( \theta_{2} + \theta_{3} \right) + a_{2}cos(\theta_{2}) \\ a_{3}\sin\left( \theta_{2} + \theta_{4} \right) + a_{2}sin(\theta_{2}) \\ - \varepsilon_{5} - \varepsilon_{4} + \varepsilon_{1} \\ \end{bmatrix}$ p_0, 5 z = −ε₅ − ε₄ + ε₁ ε₄ =  ε₁ − ε₅ − p_0, 5 z

Do wyznaczenia θ₅potrzeba jest macierz H₀⁵, właściwie jej część, którą tworzy macierz R.

$R_{0}^{5} = \begin{bmatrix} cos(\theta_{2} + \theta_{3} - \theta_{5}) & sin(\theta_{2} + \theta_{3} - \theta_{5}) & 0 \\ sin(\theta_{2} + \theta_{3} - \theta_{5}) & - cos(\theta_{2} + \theta_{3} - \theta_{5}) & 0 \\ 0 & 0 & - 1 \\ \end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \\ \end{bmatrix}$

Stosunek elementu r₂₁ do r₁₁ pozwoli wyznaczyć wartość θ₅.

$$\theta_{5} = \theta_{2} + \theta_{3} - \arctan\left( \frac{r_{21}}{r_{11}} \right)$$

Tym sposobem zostały wyznaczone wszystkie zmienne złączowe, pozwalające na osiągnięcie przez końcówkę roboczą manipulatora zadanej pozycji i orientacji.

Skrypty umożliwiające animację 3D manipulatora typu SCARA.

Funkcje rysujące model 3D manipulatora i przyporządkowane do niego układy współrzędnych.

Funkcja rysująca każdy z bloków wykorzystanych w animacji oprócz chwytaka.

function RysBlok( szerokosc, wysokosc, srednica, wierzcholki, H)

% funkcja rysuje figurę przypominającą koło

% o zadanej średnicy, wysokości, ilości wierzchołków i szerokości,

% która pozawala "rozciągnąć" koło, aby w efekcie otrzymać ramię

npi = 2*pi/wierzcholki;

n=1;

% pętla odpowiadająca za macierz współrzędnych wierzchołków

for t = 0:npi:(2*pi-npi)

if t > pi

punkty(n,1) = -szerokosc+srednica*sin(t);

else

punkty(n,1) = srednica*sin(t);

end

punkty(n,2) = srednica*cos(t);

punkty(n,3) = 0;

if t > pi

punkty(n+wierzcholki,1) = -szerokosc+srednica*sin(t);

else

punkty(n+wierzcholki,1) = srednica*sin(t);

end

punkty(n+wierzcholki,2) = srednica*cos(t);

punkty(n+wierzcholki,3) = wysokosc;

n=n+1;

end

punkty(:,4) = ones(size(punkty,1),1);

punkty = (H*(punkty'))'; % odpowiada za przesuwanie figury

plaszczyzna2(1,:) = 1:wierzcholki; % macierz płaszczyzny poziomej (góra i dół figury)

plaszczyzna2(2,:) = (wierzcholki+1):(2*wierzcholki);

for n=1:wierzcholki % pętla odpowiadająca za macierz punktów płaszczyzny pionowej

if n==wierzcholki

plaszczyzna(n,:) = [ n 2*n n+1 1 ];

else

plaszczyzna(n,:) = [ n n+1 n+wierzcholki+1 n+wierzcholki ];

end

end

for n=1:wierzcholki % pętla odpowiadająca stworzenie macierzy kolorów

kolory_p(n,:) = [ 1 0 0 ];

end

kolory_p2 = [ 0 0 1 % kolory płaszczyzna

0 0 1 ];

patch('Vertices', punkty(:,1:3), 'Faces', plaszczyzna2, 'FaceVertexCData', kolory_p2, 'FaceColor','flat');

patch('Vertices', punkty(:,1:3), 'Faces', plaszczyzna, 'FaceVertexCData', kolory_p, 'FaceColor','flat');

end

Funkcja odpowiadająca za rysowanie chwytaka.

function RysChwytak(skala, H)

% funkcja odpowiadająca za rysowanie chwytaka.

punkty = [ 1 1 -6 1

1 -1 -6 1

2 -1 -6 1

2 1 -6 1

1 1 -2 1

1 -1 -2 1

2 -1 -2 1

2 1 -2 1

-2 1 -6 1

-2 -1 -6 1

-1 -1 -6 1

-1 1 -6 1

-2 1 -2 1

-2 -1 -2 1

-1 -1 -2 1

-1 1 -2 1

2 -1 0 1

2 1 0 1

-2 1 0 1

-2 -1 0 1]; % macierz współrzędnych punktów tworzące chwytak

punkty = skala*punkty; % przeskalowanie rozmiarów chwytaka

punkty(:,4) = ones(size(punkty,1),1); % generuje macierz jedynkową o takiej samej liczbie wierszy

% jak macierz 'punkty' i dodaje ją jako czwartą kolumnę macierzy 'punkty'.

punkty = (H*(punkty'))'; %odpowiada za przesuwanie figury

plaszcz = [ 1 2 3 4

5 6 7 8

1 2 6 5

2 3 7 6

3 4 8 7

1 5 8 4

9 10 11 12

13 14 15 16

9 10 14 13

10 11 15 14

11 12 16 15

9 13 16 12

7 8 13 14

17 18 19 20

7 8 18 17

8 13 19 18

13 14 20 19

7 17 20 14];

for n=1:18 % pętla odpowiadająca stworzenie macierzy kolorów

kolory_p(n,:) = [ 0 0 0 ];

end

patch('Vertices', punkty(:,1:3), 'Faces', plaszcz, 'FaceVertexCData', kolory_p, 'FaceColor','flat');

end

Funkcja rysująca układy współrzędnych.

function rysukl(H,nazwa)

% uw - startowy układ współrzednych (wektory kolumnowe w postaci jednorodnej)

% kolumna 1 - początek układu współrzędnych,

% kolumna 2 - koniec osi X,

% kolumna 3 - koniec osi Y,

% kolumna 4 - koniec osi Z,

uw= [0 5 0 0

0 0 5 0

0 0 0 5

1 1 1 1];

uw=H*uw;

plot3([uw(1,1) uw(1,2)],[uw(2,1) uw(2,2)],[uw(3,1) uw(3,2)],'b','LineWidth',2)

%oś X - niebieski

plot3([uw(1,1) uw(1,3)],[uw(2,1) uw(2,3)],[uw(3,1) uw(3,3)],'g','LineWidth',2)

%oś Y - zielony

plot3([uw(1,1) uw(1,4)],[uw(2,1) uw(2,4)],[uw(3,1) uw(3,4)],'r','LineWidth',2)

%oś Z - czerwony

text(uw(1,2),uw(2,2),uw(3,2),nazwa,'FontSize',25)

end

Funkcja dla kinematyki prostej i odwrotnej

Funkcja kinematyki prostej

function KinematykaProsta(kat1,kat2,kat3,E)

%kat1 - pierwsze ramię, kat2 - drugie ramię, kat3 - chwytak, E - ruch drugiej kolumny,

X=[1 0 0]';

Y=[0 1 0]';

Z=[0 0 1]';

cla

H0=Trans(0,0,0);

H1=Trans(0,0,17); % macierz jednorodna układu 1

H2=Rot(Z,kat1)*Trans(15,0,0); % macierz jednorodna układu 2

H3=Rot(Z,kat2)*Trans(12,0,2.5)*Rot(X,180); % macierz jednorodna układu 3

H4=Trans(0,0,E+2.5); % macierz jednorodna układu 4

H5=Rot(Z,kat3)*Rot(X,180); % macierz jednorodna układu 5

H01=H0*H1 % macierz przejścia z układ 0 do układu 1

H02=H01*H2 % macierz przejścia z układ 1 do układu 2

H03=H02*H3 % macierz przejścia z układ 2 do układu 3

H04=H03*H4 % macierz przejścia z układ 3 do układu 4

H05=H04*H5 % macierz przejścia z układ 4 do układu 5

rysukl(H0,'0')

rysukl(H01,'1')

rysukl(H02,'2')

rysukl(H03,'3')

rysukl(H04,'4')

rysukl(H05,'5')

RysBlok( 0, 1.5, 3, 50, H0 ); %podstawa

RysBlok( 0, 17, 1.5, 20, H0 ); %pierwsza kolumna

RysBlok( 15, 1.5, 1.5, 20, H02 ); %pierwsze ramię

RysBlok( 12, 1, 1.5, 20, H03 ); %drugie ramię

RysBlok( 0, -17.5, 0.5, 10, H04 ); %druga kolumna

RysChwytak(0.3, H05); %chwytak

end

Macierz obrotu i macierz przesunięcia

function H=Rot(u, th) % funckja macierzy obrotu H

c=@cosd;

s=@sind;

R = u*u'*(1 - c(th)) + [ c(th) -u(3)*s(th) u(2)*s(th)

u(3)*s(th) c(th) -u(1)*s(th)

-u(2)*s(th) u(1)*s(th) c(th) ];

H=eye(4);

H(1:3, 1:3) = R; % przypisanie R od wiesza 1 do 3 i od kolumny 1 do 3.

end

function H=Trans(x, y, z) % funkcja przesunięcia H

H = [ 1 0 0 x

0 1 0 y

0 0 1 z

0 0 0 1 ];

end

Funkcja kinematyki odwrotnej

function KinematykaOdwrotna( x, y, z)

a1= 15; %długość pierwszego ramienia

a2= 12; %długość drugiego ramienia

D = (x*x+y*y-a1*a1-a2*a2)/(2*a1*a2); %zmienna pomocnicza, do kolejnych.

kat2 = atan2(-(sqrt(1 - D*D)), D); %zmienna, która liczy kąt zmiany przegubu pierwszego.

Beta = atan2(a2*sin(kat2), a1+a2*cos(kat2));

kat1 = atan2(y, x) - Beta; % zmienna, która liczy kąt zmiany przegubu drugiego.

E = 14 - z; % liczy przesunięcie chwytaka.

kat1 = (180*kat1)/pi

kat2 = (180*kat2)/pi

kat3 = atan2(y,x);

kat3 = kat1+kat2-kat3 % kąt zmiany chwytaka.

KinematykaProsta(kat1,kat2,kat3,E);

end

Przykładowa funkcja wizualizująca ruch manipulatora dla zadanej trajektorii

Spirala

function rysujspirala(r, wsp_x, wsp_y)

i=0;

for t = 0: 2: 500

i=i+1;

% Obliczanie wpółrzędnych trajektorii ruchu

x=r*cos(2*pi*t/100)+wsp_x;

y=r*sin(2*pi*t/100)+wsp_y;

z=0.02*t+1;

KinematykaOdwrotna( x, y, z);

% Rysowanie trajektorii

A(1,i)=x;

B(1,i)=y;

C(1,i)=z;

plot3(A,B,C,'Color',[0 0 0],'LineWidth',1.5);

drawnow

end

end

Zrzuty ekranów z przykładowymi konfiguracjami

Rysunek 1 - rysowaniu kwadratu o boku 20

Rysunek 2 - rysowanie spirali o średnicy 7,5 i środku w pkt (-8,-8)

Literatura

• Craig J. J.: Wprowadzenie do robotyki. Mechanika i sterowanie, Wydawnictwa Naukowo-Techniczne, 1995

• Jezierski E.: Dynamika robotów, Wydawnictwa Naukowo-Techniczne, 2006

• http://www.robotyka.com/teoria.php/teoria.48

• Buratowski T.: Podstawy robotyki, AGH Uczelniane Wydawnictwa Naukowo- Dydaktyczne, 2006