Mateusz Raciniewski

nr albumu: 242875

Kinematyka prosta i odwrotna manipulatora typu SCARA.

Rys. 1. Manipulator typu SCARA firmy Mitsubishi.

Rys. 2. Schemat kinematyczny z przyporządkowanymi układami współrzędnych.

	θ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

Tab. 1. Tabela D-H.

Kinematyka prosta.

Przekształcenia jednorodne są w robotyce używane do obliczania zadań prostych i odwrotnych manipulatorów robotów.

Zadanie proste polega na obliczeniu na podstawie zmiennych kinematycznych (czyli obrotów i wysuwów odpowiednich członów robota) położenia końcówki manipulatora (lub innego jego punktu) w globalnym (zewnętrznym) układzie współrzędnych.

Poniżej został przedstawiony przykład macierzy przekształcenia jednorodnego.

H- macierz przekształcenia;

R- macierz opisująca orientację w przestrzeni;

d- wektor położenia.

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

Podam teraz macierze, które pozwalają na obracanie punktu (bryły, całego układu brył), względem osi x, y, z zewnętrznego układu współrzędnych

Rot_(X,α)=$\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & cos(\alpha) & - sin(\alpha) & 0 \\ 0 & sin(\alpha) & cos(\alpha) & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}$ Rot_(Y,α)=$\begin{bmatrix} cos(\alpha) & 0 & sin(\alpha) & 0 \\ 0 & 1 & 0 & 0 \\ - sin(\alpha) & 0 & cos(\alpha) & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}$

Rot_(Z,α)=$\begin{bmatrix} cos(\alpha) & - sin(\alpha) & 0 & 0 \\ sin(\alpha) & cos(\alpha) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}$

oraz macierz przesunięcia w poszczególnych osiach x,y,z

Trans(d)=$\begin{bmatrix} 1 & 0 & 0 & d_{x} \\ 0 & 1 & 0 & d_{y} \\ 0 & 0 & 1 & d_{z} \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}$

Macierze jednorodne (⁰H₁,¹H₂…) opisujące pozycje i orientacje kolejnych układów współrzędnych przyporządkowanych do manipulatora typu SCARA względem poprzednich oraz macierz jednorodną (⁰H₅) opisującą położenie i orientację ostatniego układu współrzędnych względem układu bazowego zapisane na podstawie tabeli D-H:

⁰H₁=Trans(0,0,ε₁₎₌ $\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & \varepsilon_{1} \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}$

¹H₂=Rot(Z, θ₂)*Trans(a₂,0,0)=

$$= \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}$$

²H₃=Rot(Z, θ₃)*Trans(a₃,0,0)*Rot(X,180^o)=

$$= \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}$$

³H₄=Trans(0,0,Ɛ₄)=$\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & \varepsilon_{4} \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}$

⁴H₅=Rot(Z, θ₅)*Trans(0,0,Ɛ₅)=

$$= \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}$$

⁰H₅=(⁰H₁)*(¹H₂)*(²H₃)*(³H₄)*(⁴H₅)

⁰H₅=$\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}$

Kinematyka odwrotna.

Odwrotne zadanie kinematyki polega na wyznaczeniu wszystkich możliwych zbiorów wartości przemieszczeń kątowych i liniowych (współrzędnych konfiguracyjnych) w połączeniach ruchowych, które umożliwią manipulatorowi osiągnięcie zadanych pozycji lub orientacji członu roboczego chwytaka lub narzędzia. Jest to podstawowe zadanie programowania i sterowania ruchu manipulatora, gdy trzeba znaleźć jak poszczególne współrzędne konfiguracyjne powinny zmieniać się w czasie w celu realizacji pożądanego ruchu członu roboczego.

Mając dane pozycję i orientację należy obliczyć wszystkie możliwe zbiory współrzędnych konfiguracyjnych tak, aby osiągnąć pożądaną pozycję i orientację. Jest to zadanie trudniejsze do prostego zadania kinematyki ze względu na wielokrotność rozwiązań i ich nieliniowość.

Problem kinematyki odwrotnej można przedstawić następująco: dana jest jednorodna macierz przekształcenia 4x4

H=$\begin{bmatrix} R & p \\ 0 & 1 \\ \end{bmatrix}$=$\begin{bmatrix} h_{11} & h_{12} & h_{13} & h_{14} \\ h_{21} & h_{22} & h_{23} & h_{24} \\ h_{31} & h_{32} & h_{33} & h_{34} \\ h_{41} & h_{42} & h_{43} & h_{44} \\ \end{bmatrix}$

należy znaleźć jedno lub wszystkie rozwiązania równania

⁰H_n(q₁,q₂,…,q_n)=H (1)

gdzie:

⁰H_n(q₁,q₂,…,q_n)=⁰H₁(q₁)¹H₂(q₂)…^n-1H_n(q_n)=$\ \begin{bmatrix} H_{11} & H_{12} & H_{13} & H_{14} \\ H_{21} & H_{22} & H_{23} & H_{24} \\ H_{31} & H_{32} & H_{33} & H_{34} \\ H_{41} & H_{42} & H_{43} & H_{44} \\ \end{bmatrix}$

Równanie (1) daje 12 równań z n niewiadomymi, które można zapisać jako

H_ij(q₁,q₂,…,q_n)=h_ij; i=1,2,3; j=1,2,3,4

gdzie: H_ij, h_ij oznaczają 12 nietrywialnych elementów macierzy, odpowiednio ⁰H_n i H.

Ponieważ ostatni wiersz obu macierzy jest równy (0,0,0,1), więc spośród 16 równań reprezentowanych przez zależność (1) cztery są trywialne.

Zadanie kinematyki odwrotnej można próbować rozwiązać na wiele sposobów np. wykorzystując podejście geometryczne, analityczne numeryczne.

Kinematyka odwrotna dla manipulatora typu SCARA została wyprowadzona poniżej.

Rys.3. Zrzutowanie ramion manipulatora na płaszczyznę x₀y₀.

Pierwsze 2 złącza θ₂ i θ₃ można wyznaczyć jak w przypadku manipulatora płaskiego z łokciem.

Rys.4. Dwuczłonowy manipulator płaski z łokciem.

W przypadku manipulatora SCARA należy jedynie zmienić oznaczenia:

θ₁ = θ₂; θ₂ = θ₃; a₁=a₂; a₂=a_3.

Stosując prawo cosinusów, można wyznaczyć kąt θ₃ w następujący sposób:

(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$ (2)

θ₃ = arccosD

jeżeli cosθ₃ jest dany wzorem (2) to sinθ₃ wynosi:

$$\sin\theta_{3} = \pm \sqrt{1 - D^{2}}$$

stąd:

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

Na podstawie rysunku 4 można zapisać że:

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

θ₂ = α − β

Stosując prawo sinusów można natomiast wyznaczyć kąt β:

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

zatem:

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

W wyprowadzonych wzorach należy użyć funkcji atan2(y,x), która oblicza wartość z uwzględnieniem znaków zarówno przy x jak i y, w celu znalezienia ćwiartki, w której leży szukany kąt. Ostateczne wzory dla dwóch pierwszych złącz manipulatora typu SCARA wyglądają następująco:

$D = \frac{x^{2} + y^{2} - a_{2}^{2} - a_{3}^{2}}{2a_{2}a_{3}}$ (3a)

θ₂ = atan2(y,x) − atan2(a₃sinθ₃,a₂+a₃cosθ₃) (3b)

$\theta_{3} = atan2\left( \pm \sqrt{1 - D^{2}},D \right)$ (3c)

Należy jeszcze wyznaczyć ε₄ oraz θ₅. ε₄ można wyznaczyć w następujący sposób:

$\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}$

więc:

p_0, 5 z=−ε₅ − ε₄ + ε₁

zatem:

ε₄=ε₁ − ε₅− p_0, 5 z (4)

Kąt θ₅również można wyznaczyć używając macierzy ⁰H₅.

⁰H₅=$\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}$

R=$\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}$

Na podstawie powyższego równania wynika, że:

$$\frac{sin(\theta_{2} + \theta_{3} - \theta_{5})}{cos(\theta_{2} + \theta_{3} - \theta_{5})} = \frac{r_{21}}{r_{11}}$$

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

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

 θ₅=$\theta_{2} + \theta_{3} - arctg(\frac{r_{21}}{r_{11}})$ (5)

Powyższe wzory uzyskane w kinematyce prostej i odwrotnej wykorzystałem do napisania skryptu w programie Matlab, który umożliwia wizualizację manipulatora. Umieszczam zrzuty ekranów dla poszczególnych konfiguracji manipulatora.

Rys.5. Zerowe wartości zmiennych złączowych.

Rys.6. Zmienne złączowe równe 90.

Rys.7. Złącze drugie równe 165.

Literatura:

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

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

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

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