Laboratorium Podstaw Robotyki

Prowadzący: mgr inż. Igor Zubrycki

Poniedziałek (10:15-11:45). Kierunek Automatyka i Robotyka

GR 1

Sprawozdanie z zajęć laboratoryjnych

Modelowanie robota o 6 stopniach swobody

TX90

Skład zespołu:

Blicharz Robert 187229

Bodura Andrzej 187231

Gołygowski Damian 187247

Stroka Jakub 200159

Wprowadzenie

Zajęcia laboratoryjne poświęcone były zapoznaniu się z budową wybranego robota z manipulatorem o 6 stopniach swobody oraz zamodelowaniu jego budowy w oraz kinematyki w programie Matlab przy użyciu Robotics Toolbox by Peter Corke i w języku URDF (Unified Robot Description Format).

Przedmiotem naszego działania był robot firmy Staubli serii TX90 z manipulatorem o 6 stopniach swobody i o dość małych wymiarach fizycznych.

Rysunek 1 - Widok 2D manipulatora z wymiarami Rysunek 2 Widok 3D

Notacja Denavita-Hartenberga

Pierwszym poważnym wyzwaniem było sporządzenie tablicy D-H opisującej relacje pomiędzy kolejnymi układami współrzędnych przypisanymi do złączy manipulatora.

Odpowiednie przygotowanie notacji wymagało sporządzenia uproszczonego rysunku manipulatora, na którym miałby się znajdować wyłącznie wymiary, które są niezbędne do opisu manipulatora. Odrzuciliśmy więc wszystkie wymiary poza tymi długościami kolejnych elementów łańcucha kinematycznego. Wszystkie wymiary zostały pobrane z opisu technicznego TX90.

Uproszczony rysunek przedstawiający jedynie niezbędne wymiary manipulatora

Rysunek 3 Uproszczony rysunek manipulatora

Następnie na podstawie powyższego rysunku oraz znając sposób tworzenia notacji Denavita-Hartenberga przygotowaliśmy tabelę podzieloną odpowiednio na:
obrót wokół osi Z (theta), przesunięcie wzdłuż osi Z (D), przesunięcie wzdłuż osi X (A) oraz obrót wokół osi X (alpha).

Tabela D-H

	Theta (Ѳ)	D	A	Alpha (α)
1	Theta1	0.478	0,050	-π/2
2	Theta2	0.050	0.425	0
3	Theta3	0	0	π/2
4	Theta4	0.425	0	-π/2
5	Theta5	0	0	π/2
6	Theta6	0.100	0	0

Zmienna o nazwie Theta i kolejnych indeksach numerowanych od 1 do 6 oznacza dowolny (jednak w pewnych granicach) obrót wokół osi Z. Tablice D-H zapisuje się tak, aby wszelkie obroty następowały zawsze wokół osi Z. Wymiary kątów podane są w radianach a przesunięcia w metrach.

Pierwszy model w URDF

Po stworzeniu notacji D-H oraz rysunku manipulatora kolejnym krokiem było zapoznanie się z UDRF i stworzenie modelu obrazującego rzeczywisty wygląd manipulatora. W tym celu posłużyliśmy się tutorialem ze strony Gentle.pl, w którym krok po kroku został przedstawiony sposób tworzenia modelu.

Modelowanie manipulatora przeprowadzaliśmy na stronie internetowej mymodelrobot.appspot.com

Rysunek 4 Podgląd pierwszego modelu TX90 w URDF

Budowanie robota w URDF polega na operowaniu składnią oznaczoną tagiem:

<robot name="nazwa"></robot>

Pomiędzy tymi tagami umieszcza się jeszcze dodatkowe, które odpowiadają za wygląd, wymiar i położenie elementu łańcucha kinematycznego <link></link> oraz tag odpowiedzialny za połączenie dwóch elementów łańcucha <joint name="nazwa" type="typ-złącza></joint>.

W tym tagu dodatkowo można także określić typ połączenia (revolute/fixed - obrotowe/stałe "zespawane"), oś obrotu np. <axis xyz="0 0 1"/>

W tagach <parent link="nazwa_rodzica"/> <child link="nazwa_dziecka"/> należy określić które elementy mają zostać ze sobą połączone.

Tag <origin xyz="0 0 0.252" /> pozwala na określenie położenia punktu łączenia dwóch elementów łańcucha kinematycznego.

W ty modelu odwzorowaliśmy fizyczną budowę robota, nie odnosząc się zasadniczo do tabeli DH

Kod odpowiadający pierwszemu modelowi manipulatora w URDF

<?xml version="1.0"?>

<robot name="staubli">

<link name="podstawa">

<visual>

<geometry>

<cylinder length="0.132" radius="0.130"/>

</geometry>

</visual>

</link>

<link name="zlacze_1_obrotowe">

<visual>

<geometry>

<cylinder length="0.346" radius="0.125"/>

</geometry>

<material name="czerwony">

<color rgba="1 0 0 1"/>

</material>

</visual>

</link>

<joint name="1_zlacze_obrotowe" type="revolute">

<parent link="podstawa"/>

<child link="zlacze_1_obrotowe"/>

<origin xyz="0 0 0.252" />

<axis xyz="0 0 1"/>

<limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0.5"/>

</joint>

<link name="zlacze_2_obrotowe">

<visual>

<geometry>

<cylinder length="0.035" radius="0.120"/>

</geometry>

<origin rpy="1.57 1.57 0" xyz="-0.145 0.05 0"/>

<material name="zielone">

<color rgba="0 1 0 1"/>

</material>

</visual>

</link>

<joint name="polacz1" type="fixed">

<parent link="zlacze_1_obrotowe"/>

<child link="zlacze_2_obrotowe"/>

<origin xyz="0 0 0.053"/>

</joint>

<link name="ramie1_obrotowe">

<visual>

<geometry>

<cylinder length="0.143" radius="0.120"/>

</geometry>

<origin rpy="1.57 1.57 0" xyz="-0.233 0 0"/>

<material name="niebieski">

<color rgba="0 0 1 1"/>

</material>

</visual>

</link>

<joint name="2_zlacze_obrotowe" type="revolute">

<parent link="zlacze_2_obrotowe"/>

<child link="ramie1_obrotowe"/>

<origin xyz="0 0.05 0" />

<axis xyz="1 0 0"/>

<limit effort="1000.0" lower="-2.2689" upper="2.57436" velocity="0.5"/>

</joint>

<link name="ramie">

<visual>

<geometry>

<box size="0.143 0.143 0.425"/>

</geometry>

<material name="niebieskie">

<color rgba="0 0 1 1"/>

</material>

</visual>

</link>

<joint name="polacz_ramie" type="fixed">

<parent link="ramie1_obrotowe"/>

<child link="ramie"/>

<origin xyz="-0.233 0 0.2145"/>

</joint>

<link name="ramie1_obrotowe2">

<visual>

<geometry>

<cylinder length="0.143" radius="0.120"/>

</geometry>

<origin rpy="1.57 1.57 0" xyz="0 0 0"/>

<material name="niebieski">

<color rgba="0 0 1 1"/>

</material>

</visual>

</link>

<joint name="polacz_ramie2" type="fixed">

<parent link="ramie"/>

<child link="ramie1_obrotowe2"/>

<origin xyz="0 0 0.2145"/>

</joint>

<link name="zlacze_3_obrotowe">

<visual>

<geometry>

<cylinder length="0.300" radius="0.096"/>

</geometry>

<material name="czerwony">

<color rgba="1 0 0 1"/>

</material>

</visual>

</link>

<joint name="zlacze_3_obrotowe" type="revolute">

<parent link="ramie1_obrotowe2"/>

<child link="zlacze_3_obrotowe"/>

<origin xyz="0.143 0 0"/>

<axis xyz="1 0 0"/>

<limit effort="1000.0" lower="-2.5307" upper="2.5307" velocity="0.5"/>

</joint>

<link name="3_zlacze_obrotowe">

<visual>

<geometry>

<cylinder length="0.216" radius="0.080"/>

</geometry>

<material name="Cyan1">

<color rgba="0 0.9 0.9 1.0"/>

</material>

</visual>

</link>

<joint name="4_zlacze_obrotowe" type="revolute">

<parent link="zlacze_3_obrotowe"/>

<child link="3_zlacze_obrotowe"/>

<origin xyz="0 0 0.252" />

<axis xyz="0 0 1"/>

<limit effort="1000.0" lower="-4.7124" upper="4.7124" velocity="0.5"/>

</joint>

<link name="zlacze_5_obrotowe">

<visual>

<geometry>

<cylinder length="0.150" radius="0.059"/>

</geometry>

<origin rpy="1.57 1.57 0" xyz="0 0 0"/>

<material name="Yellow2">

<color rgba="0.8 0.8 0 1.0"/>

</material>

</visual>

</link>

<joint name="polacz_zlacze_4" type="revolute">

<parent link="3_zlacze_obrotowe"/>

<child link="zlacze_5_obrotowe"/>

<origin xyz="0 0 0.167"/>

<axis xyz="1 0 0"/>

<limit effort="1000.0" lower="-2.0071" upper="2.4435" velocity="0.5"/>

</joint>

<link name="5_zlacze_obrotowe">

<visual>

<geometry>

<cylinder length="0.049" radius="0.031"/>

</geometry>

<origin rpy="0 0 0" xyz="0 0 0.0835"/>

<material name="Black1">

<color rgba="0.2 0.2 0.2 1.0"/>

</material>

</visual>

</link>

<joint name="5_zlacze_obrotowe" type="revolute">

<parent link="zlacze_5_obrotowe"/>

<child link="5_zlacze_obrotowe"/>

<origin xyz="0 0 0" />

<axis xyz="0 0 1"/>

<limit effort="1000.0" lower="-4.7124" upper="4.7124" velocity="0.5"/>

</joint>

</robot>

Poniższy rysunek przedstawia sposób w jaki kolejne tagi opisują elementy łańcucha kinematycznego manipulatora. Można z niego odczytać jakie relacje zachodzą pomiędzy elementami kodu.

Rysunek zaczerpnięty ze strony: http://wiki.ros.org/urdf/Tutorials/Create%20your%20own%20urdf%20file

Rysunek 5 Graficzne przedstawienie sposobu opisywania manipulatora za pomocą URDF

Modelowanie robota w programie Matlab

Kolejnym krokiem po utworzeniu opisu URDF było zamodelowanie manipulatora w środowisku programu Matlab przy użyciu toolbox'a napisanego przez Peter'a Corke'a w wersji 9.8, który pozwala m.in na obliczenie kinematyki prostej i odwrotnej oraz nadanie trajektorii ruchu manipulatorowi. Zamodelowanie oznacza odwzorowanie rzeczywistych właściwości za pomocą opisu matematycznego.

Po udanym zainstalowaniu toolbox'a czyli wkopiowaniu folderu rvctools do folderu toolbox w katalogu głównym Matlaba polecenie startup_rvc(); uruchomi Robotics Toolbox.

Pierwszym krokiem aby wyświetlić robota jest implementacja tablicy D-H.

Odbywa się to analogicznie do deklarowania macierzy. Przy implementowaniu tablicy D-H należy dodać jedną kolumnę na końcu tablicy, zawierającą informacje na temat rodzaju złącza. Odpowiednio 0 - złącze obrotowe, 1- złącze pryzmatyczne (przesuwne)

W programie Matlab macierz deklaruje się w następujący sposób

nazwa_tablicy=[wyraz1, wyraz2, wyraz3];

theta1=0;

theta2=0;

theta3=0;

theta4=0;

theta5=0;

theta6=0;

dh=[theta1, 0.478, 0.050, -pi/2, 0;

theta2, 0.050, 0.425, 0, 0;

theta3, 0, 0, pi/2, 0;

theta4, 0.425, 0, -pi/2, 0;

theta5, 0, 0, pi/2, 0;

theta6, 0.100, 0, 0, 0;]

Przed zadeklarowaniem macierzy z D-H należało także przypisać zmiennym złączowym wartości początkowe. W naszym przypadku zera. Przy wyświetlaniu robota właśnie te wartości zostaną nadane złączom manipulatora.

Stworzenie obiektu - łańcucha kinematycznego, poleceniem SerialLink. Staubli w naszym przypadku to nazwa robota, której używaliśmy w funkcji Matlaba.

staubli=SerialLink(dh)

Deklaracja wektora zmiennych złączowych. Zawiera on wartości zmiennych przypisane im przed deklaracją tablicy D-H

poloz_zlacz=[theta1, theta2, theta3, theta4, theta5, theta6]

Wyświetlanie robota z narzuconymi im wartościami zmiennych złączowych.

figure

staubli.plot(poloz_zlacz)

Złącza manipulatora rzadko mają zakres obrotu +/- 180 stopni, więc należy narzucić zakres wartości jakie dane złącze może przyjmować. Do tego celu służy polecenie: nazwa_robota.glim(nr_złącza,:)=[min,max]

staubli.qlim(1,:)=[-pi,(8/9)*pi]; %-180 +160

staubli.qlim(2,:)=[-pi*(13/18),(59/72)*pi]; %-130 +147,5

staubli.qlim(3,:)=[-pi*(29/36),pi*(29/36)]; %-145 +145

staubli.qlim(4,:)=[-pi*(3/2),pi*(3/2)]; %-270 +270

staubli.qlim(5,:)=[-pi*(23/36),pi*(7/9)]; %-115 +140

staubli.qlim(6,:)=[-pi*(3/2),pi*(3/2)]; %-270 +270

Polecenie teach() wyświetla nam dodatkowo okienko z suwakami, dzięki którym możemy dowolnie zmieniać wartości zmiennych złączowych bez potrzeby ich ponownego deklarowania w kodzie,

staubli.teach()

Rysunek 6 Manipulator w położeniu początkowym

Kinematyka prosta

Toolbox potrafi wyliczyć macierz homogeniczną podpowiadającą położeniu końcówki manipulatora w trójwymiarowej przestrzeni kartezjańskiej na podstawie wartości zmiennych złączowych. Proces ten nazywa się kinematyką prostą.

Przy pomocy Robotics Toolbox ten proces ogranicza się do użycia jednej funkcji, której parametrem wejściowym jest wektor wartości zmiennych złączowych.

macierzk=staubli.fkine(poloz_zlacz)

Funkcja zwraca macierz K, która tak jak napisano wyżej, odpowiada położeniu końcówki manipulatora w przestrzeni.

Polecenie trplot() wyświetla układ współrzędnych wygenerowany z macierzy K "przyczepiony" do końcówki manipulatora.

trplot(macierzk)

Przy zadeklarowaniu wartości zmiennych złączowych kinematyka prosta zostaje wyliczona automatycznie.

Drugi model w URDF

Drugi model w URDFie miał odwzorowywać tablicę DH oraz manipulator zamodelowany w Matlabie, oraz wykonywać taką samą trajektorię.

Kod odpowiadający drugiemu modelowi manipulatora w URDF

<?xml version="1.0"?>

<robot name="staubli">

<link name="baza">

<visual>

<geometry>

<cylinder length="0.1" radius="0.5"/>

</geometry>

<origin xyz="0 0 -0.05" />

</visual>

</link>

<joint name="zlacze_0" type="revolute">

<parent link="baza"/>

<child link="ogniwo_1"/>

<origin rpy="0 0 0" xyz="0 0 0" />

<axis xyz="0 0 1"/>

<limit effort="1000.0" lower="-2.267" upper="2.573" velocity="6.98"/>

</joint>

<link name="ogniwo_1">

<visual>

<geometry>

<cylinder length="0.481" radius="0.01"/>

</geometry>

<material name="różowa_pantera">

<color rgba="1 0 1 1"/>

</material>

<origin rpy="0 0.103 0" xyz="0.025 0 0.2405" /> //dzieki temu przesunieciu poczatek ogniwa pokrywa sie z poczatkiem u. wsp.

</visual>

</link>

<link name="ogniwo_stale">

<visual>

<geometry>

<cylinder length="0.05" radius="0.01"/>

</geometry>

<origin xyz="0 0 0.025"/>

<material name="niebieski">

<color rgba="0 0 1 1"/>

</material>

</visual>

</link>

<joint name="zlacze_1" type="revolute">

<parent link="ogniwo_1"/>

<child link="ogniwo_stale"/>

<origin rpy="-1.57 0 0" xyz="0.05 0 0.478" />

<axis xyz="0 0 1"/>

<limit effort="1000.0" lower="-3.837" upper="1.003" velocity="6.98"/>

</joint>

<link name="ogniwo_2">

<visual>

<geometry>

<cylinder length="0.425" radius="0.01"/>

</geometry>

<origin rpy="0 0 0" xyz="0 0 0.2125"/>

<material name="zielone">

<color rgba="0 1 0 1"/>

</material>

</visual>

</link>

<joint name="zlacze_stale" type="fixed">

<parent link="ogniwo_stale"/>

<child link="ogniwo_2"/>

<origin rpy="0 1.57 -1.57" xyz="0 0 0.05" />

<axis xyz="0 0 1"/>

<limit effort="1000.0" lower="-2.267" upper="2.573" velocity="6.98"/>

</joint>

<link name="ogniwo_3">

<visual>

<geometry>

<cylinder length="0.425" radius="0.01"/>

</geometry>

<origin xyz="0 0 0.2125"/>

<material name="niebieski">

<color rgba="0 0 1 1"/>

</material>

</visual>

</link>

<joint name="zlacze_2" type="revolute">

<parent link="ogniwo_2"/>

<child link="ogniwo_puste"/>

<origin rpy="1.57 0 -1.57" xyz="0 0 0.425" />

<axis xyz="0 0 1"/>

<limit effort="1000.0" lower="-0.959" upper="4.099" velocity="6.98"/>

</joint>

--ogniwa puste wprowadziliśmy aby umożliwić umieszczenie dwóch złączy

--w jednym punkcie przy zachowaniu struktury łańcucha kinematycznego

<link name="ogniwo_puste">

</link>

<joint name="zlacze_3" type="revolute">

<parent link="ogniwo_puste"/>

<child link="ogniwo_3"/>

<origin rpy="-1.57 0 1.57" xyz="0 0 0" />

<axis xyz="0 0 1"/>

<limit effort="1000.0" lower="-4.71" upper="4.71" velocity="7.5"/>

</joint>

<link name="ogniwo_4">

<visual>

<geometry>

<cylinder length="0.100" radius="0.01"/>

</geometry>

<origin xyz="0 0 0.05"/>

<material name="zielone">

</material>

</visual>

</link>

<joint name="zlacze_4" type="revolute">

<parent link="ogniwo_3"/>

<child link="ogniwo_puste_2"/>

<origin rpy="-1.57 0 0" xyz="0 0 0.425" />

<axis xyz="0 0 1"/>

<limit effort="1000.0" lower="-2.006" upper="2.442" velocity="9.42"/>

</joint>

<link name="ogniwo_puste_2">

</link>

<joint name="zlacze_5" type="revolute">

<parent link="ogniwo_puste_2"/>

<child link="ogniwo_4"/>

<origin rpy="1.57 0 0" xyz="0 0 0" />

<axis xyz="0 0 1"/>

<limit effort="1000.0" lower="-4.71" upper="4.71" velocity="8.286"/>

</joint>

</robot>

Rysunek 7 Podgląd drugiego modelu TX90 w URDF

Powyższy model trajektorie wykonuje dzięki plikowi .csv wygenerowanemu na podstawie zmiennych złączowych wyliczonych w za pomocą rozwiązania zadania kinematyki odwrotnej. Ze względu, że wartości dwóch z tych zmiennych znacznie wychodziły poza zakres ruchomości złącza ręcznie skorygowaliśmy plik odejmując od tych zmiennych wartość 6.28

time,zlacze_0,zlacze_1,zlacze_2,zlacze_3,zlacze_4,zlacze_5

0.00,-1.67E-14,0.3620,0.4403,3.1416,-0.4897,3.1416

0.10101,0.0002144,0.3621,0.44,3.1412,-0.4897,3.1411

0.20202,0.00085764,0.3625,0.4391,3.1401,-0.4896,3.1398

0.30303,0.0019302,0.3633,0.4377,3.1383,-0.4894,3.1376

0.40404,0.0034329,0.3642,0.4357,3.1357,-0.4892,3.1344

0.50505,0.0053662,0.3655,0.4332,3.1325,-0.4889,3.1303

0.60606,0.0077319,0.3670,0.4301,3.1285,-0.4885,3.1253

0.70707,0.010531,0.3688,0.4265,3.1238,-0.4881,3.1194

0.80808,0.013766,0.3709,0.4224,3.1185,-0.4875,3.1125

0.90909,0.017437,0.3731,0.4178,3.1125,-0.4869,3.1045

1.0101,0.021549,0.3757,0.4127,3.1059,-0.4862,3.0956

1.1111,0.026102,0.3784,0.4072,3.0987,-0.4854,3.0857

1.2121,0.031099,0.3813,0.4012,3.0909,-0.4845,3.0746

1.3131,0.036544,0.3845,0.3949,3.0825,-0.4835,3.0625

1.4141,0.042438,0.3878,0.3882,3.0737,-0.4824,3.0492

1.5152,0.048786,0.3913,0.3812,3.0643,-0.4812,3.0347

1.6162,0.05559,0.3949,0.3739,3.0545,-0.4798,3.019

1.7172,0.062853,0.3987,0.3664,3.0443,-0.4784,3.0021

1.8182,0.07058,0.4026,0.3586,3.0337,-0.4768,2.9839

1.9192,0.078772,0.4066,0.3506,3.0228,-0.4751,2.9643

2.0202,0.087435,0.4107,0.3424,3.0115,-0.4733,2.9433

2.1212,0.09657,0.4148,0.3342,3.0001,-0.4714,2.9209

2.2222,0.10618,0.4190,0.3259,2.9885,-0.4693,2.897

2.3232,0.11627,0.4231,0.3175,2.9767,-0.4671,2.8715

2.4242,0.12685,0.4273,0.3091,2.9648,-0.4648,2.8445

2.5253,0.13791,0.4315,0.3008,2.9529,-0.4624,2.8158

2.6263,0.14946,0.4356,0.2926,2.9411,-0.4599,2.7854

2.7273,0.16151,0.4397,0.2845,2.9293,-0.4573,2.7532

2.8283,0.17405,0.4436,0.2766,2.9177,-0.4547,2.7192

2.9293,0.18708,0.4475,0.2689,2.9062,-0.452,2.6833

3.0303,0.20062,0.4512,0.2614,2.895,-0.4493,2.6455

3.1313,0.21466,0.4547,0.2543,2.8842,-0.4466,2.6057

3.2323,0.22921,0.4580,0.2475,2.8737,-0.4439,2.5639

3.3333,0.24426,0.4611,0.2412,2.8636,-0.4412,2.5199

3.4343,0.25959,0.4639,0.2353,2.8541,-0.4388,2.4746

3.5354,0.27496,0.4663,0.2302,2.8454,-0.4365,2.4285

3.6364,0.29036,0.4685,0.2256,2.8373,-0.4344,2.3818

3.7374,0.30581,0.4702,0.2217,2.8299,-0.4326,2.3344

3.8384,0.32128,0.4717,0.2184,2.823,-0.4311,2.2864

3.9394,0.33679,0.4727,0.2157,2.8168,-0.4299,2.2379

4.0404,0.35233,0.4735,0.2136,2.811,-0.4289,2.189

4.1414,0.36789,0.4739,0.2122,2.8056,-0.4283,2.1396

4.2424,0.38348,0.4739,0.2113,2.8006,-0.428,2.0899

4.3434,0.39909,0.4737,0.2111,2.7959,-0.4281,2.04

4.4444,0.41472,0.4730,0.2115,2.7914,-0.4285,1.9899

4.5455,0.43036,0.4721,0.2125,2.787,-0.4293,1.9397

4.6465,0.44601,0.4707,0.2142,2.7827,-0.4305,1.8896

4.7475,0.46167,0.4691,0.2165,2.7783,-0.432,1.8394

4.8485,0.47734,0.4671,0.2194,2.7739,-0.4339,1.7895

4.9495,0.49302,0.4647,0.2229,2.7692,-0.4362,1.7398

5.0505,0.50869,0.4620,0.2271,2.7644,-0.4389,1.6904

5.1515,0.52436,0.4589,0.232,2.7592,-0.4419,1.6414

5.2525,0.54002,0.4555,0.2376,2.7536,-0.4452,1.5929

5.3535,0.55567,0.4517,0.2438,2.7475,-0.4489,1.5449

5.4545,0.57131,0.4475,0.2507,2.7409,-0.4529,1.4975

5.5556,0.58693,0.4430,0.2583,2.7338,-0.4573,1.4507

5.6566,0.60252,0.4380,0.2667,2.726,-0.4619,1.4047

5.7576,0.61809,0.4328,0.2758,2.7176,-0.4668,1.3595

5.8586,0.63362,0.4271,0.2856,2.7085,-0.472,1.3151

5.9596,0.64912,0.4210,0.2963,2.6986,-0.4774,1.2715

6.0606,0.66458,0.4146,0.3077,2.688,-0.483,1.2288

6.1616,0.67999,0.4077,0.3199,2.6766,-0.4888,1.1871

6.2626,0.69535,0.4005,0.333,2.6644,-0.4949,1.1463

6.3636,0.71066,0.3928,0.347,2.6514,-0.5011,1.1065

6.4646,0.7259,0.3846,0.3618,2.6375,-0.5074,1.0678

6.5657,0.74108,0.3761,0.3776,2.6228,-0.5138,1.03

6.6667,0.75618,0.3670,0.3943,2.6072,-0.5204,0.99338

6.7677,0.77097,0.3577,0.4118,2.591,-0.5269,0.95834

6.8687,0.78524,0.3482,0.4297,2.5745,-0.5333,0.92542

6.9697,0.79897,0.3385,0.448,2.5577,-0.5396,0.89454

7.0707,0.81218,0.3287,0.4666,2.5408,-0.5456,0.86561

7.1717,0.82486,0.3189,0.4854,2.5238,-0.5515,0.83858

7.2727,0.83703,0.3089,0.5045,2.5067,-0.5571,0.81336

7.3737,0.84868,0.2989,0.5238,2.4896,-0.5626,0.78989

7.4747,0.85982,0.2890,0.5432,2.4726,-0.5678,0.7681

7.5758,0.87046,0.2790,0.5626,2.4557,-0.5728,0.74791

7.6768,0.88061,0.2690,0.582,2.439,-0.5775,0.72927

7.7778,0.89026,0.2592,0.6014,2.4226,-0.5821,0.7121

7.8788,0.89943,0.2494,0.6206,2.4064,-0.5864,0.69634

7.9798,0.90812,0.2397,0.6397,2.3906,-0.5904,0.68192

8.0808,0.91634,0.2302,0.6585,2.3751,-0.5942,0.66878

8.1818,0.92409,0.2209,0.677,2.3601,-0.5978,0.65686

8.2828,0.93138,0.2118,0.6951,2.3455,-0.6011,0.64609

8.3838,0.93822,0.2030,0.7127,2.3315,-0.6043,0.63641

8.4848,0.94461,0.1944,0.7298,2.318,-0.6072,0.62776

8.5859,0.95057,0.1861,0.7463,2.3051,-0.6098,0.62007

8.6869,0.95608,0.1782,0.7622,2.2928,-0.6123,0.61327

8.7879,0.96116,0.1707,0.7773,2.2813,-0.6145,0.60731

8.8889,0.96582,0.1636,0.7915,2.2705,-0.6166,0.60212

8.9899,0.97006,0.1569,0.8049,2.2604,-0.6184,0.59765

9.0909,0.97388,0.1508,0.8172,2.2511,-0.6201,0.59382

9.1919,0.97728,0.1452,0.8285,2.2427,-0.6215,0.59058

9.2929,0.98028,0.1401,0.8387,2.2352,-0.6228,0.58787

9.3939,0.98287,0.1357,0.8476,2.2286,-0.6239,0.58565

9.4949,0.98506,0.1318,0.8554,2.223,-0.6248,0.58386

9.596,0.98684,0.1287,0.8617,2.2183,-0.6255,0.58245

9.697,0.98823,0.1262,0.8668,2.2147,-0.6261,0.5814

9.798,0.98922,0.1244,0.8704,2.212,-0.6265,0.58067

9.899,0.98981,0.1233,0.8726,2.2105,-0.6267,0.58024

10.00,0.99001,0.1229,0.8733,2.2099,-0.6268,0.5801

Tworzenie trajektorii ruchu manipulatora

Tworzenie trajektorii polega tutaj na wyznaczeniu odpowiedniego obrotu dla jednego lub kilku złączy manipulatora. W naszym przypadku, dla uproszczenia, obrót narzuciliśmy tylko jednemu złączu obrotowemu.

Tworzenie trajektorii oznacza wyliczanie kinematyko prostej dla każdego punktu z narzuconego zakresu dla całego zakresu obrotu złącza.

W naszym przypadku płynność ruchu jest zapewniona przez ilość klatek animacji. 400 zapewnia płynny ruch.

ileklatek=400;

Poniżej przedstawiony jest sposób w jaki zadeklarowaliśmy ruch w danym złączu. Funkcja linspace() przyjmuje jako parametry wejściowe zakres ruchu oraz ilość punktów w których policzona zostanie kinematyka prosta.

obrotwe_1=linspace(-pi/2,(pi/2),ileklatek);

obrotwe_2=0*ones(1,ileklatek);

obrotwe_3=0*ones(1,ileklatek);

obrotwe_4=0*ones(1,ileklatek);

obrotwe_5=0*ones(1,ileklatek);

obrotwe_6=0*ones(1,ileklatek);

Poniższa macierz jako wyrazy przyjmuje wartości opisane wyżej dodatkowo transponowane.

macierz_wartosci_zlacz=[obrotwe_1',obrotwe_2',obrotwe_3',obrotwe_4',obrotwe_5',obrotwe_6'];

Dla tej macierzy, czyli dla 400 punktów z zakresu ruchu zostanie policzona kinematyka prosta. Powstałe punkty stworzą trajektorię.

klatki=staubli.fkine(macierz_wartosci_zlacz);

polozenia=klatki(1:3,4,:); %wyodrebnia pierwsze trzy elementy 4 kolumny kazdej z klatek macierzy

polozeniamacierz=reshape(polozenia(:),3,ileklatek); %przeksztalca wynik w macierz o ileklatek wierszach i 3 kolumnach

%zatem 1 kolumna odpowiada x, druga y, trzecia z

polozeniamacierz=polozeniamacierz'; %transpozycja

%%

Poniższy fragment kodu wyświetla powstałą trajektorię jako krzywą na wykresie 3D.

plot3(polozeniamacierz(:,1),polozeniamacierz(:,2),polozeniamacierz(:,3))

xlabel('x')

ylabel('y')

zlabel('z')

hold on

figure

staubli.plot([0,0,0,0,0,0])

Kinematyka odwrotna

Kolejnym punktem realizowanym na laboratorium było obliczanie kinematyki odwrotnej manipulatora Staubli TX90. Polegało to na wskazaniu punktu, do którego miałby dotrzeć manipulator. W tym momencie pojawiał się problem związany z możliwym położeniem tego punktu, ponieważ manipulator ma ograniczone zakresy ruchu w złączach. Oznacza to, że manipulator ma ograniczony zasięg i nie każdy wskazany punkt może zostać odnaleziony.

Dodatkowo poważnym problem są konfiguracje osobliwe. Są to takie wartości wewnętrznych manipulatora przy osiągnięciu których układ zaczyna zachowywać się w sposób nie zamierzony.

Jeśli manipulator znajdzie w się w konfiguracji osobliwej, nie będzie w stanie samodzielnie się z niej wydostać mimo ciągłej pracy zespołów napędowych. Dla konfiguracji osobliwych jakobian jest równy zero.

Biorąc pod uwagę wszystkie powyższe informacje, szukaliśmy odpowiedniego punktu, dla którego zdanie było jak najlepiej uwarunkowane czyli szukaliśmy punktu w przestrzeni (konfiguracji manipulatora) dla którego jakobian miał jak największą wartość.

Bezpośrednie odnalezienie położenia pożądanego było dość niewygodne do zrealizowanie w sposób ręczny, ponieważ wymagało to obliczania macierzy homogenicznej. Aby uprościć ten proces, użyliśmy już gotowej części kodu, która wyświetla wyświetla okno z suwakami do ręcznego zmieniania wartości zmiennych złączowych co przekłada się na zmianę konfiguracji manipulatora. Nowe wartości zmiennych złączowych odpowiadające poszukiwanej konfiguracji przepisywaliśmy jako początkowe ustawnie zmiennych złączowych a następnie algorytm generował nam nową macierz homogeniczną K, której następnie używaliśmy jako położenie porządne dla funkcji liczącej kinematykę odwrotną.

Funkcja realizująca kinematykę odwrotną to ikine(). Jej parametry to właśnie pożądane położenie końcówki manipulatora, położenie początkowe manipulatora oraz maska określająca ilość złącz. Ostatni element polecenia w przypadku naszego manipulatora o 6 złączach jest opcjonalny.

porzadanepoloz=[0.2752 -0.0000 0.9614 0.8482;

0.0000 1.0000 0.0000 0.0500;

-0.9614 0.0000 0.2752 0.6514;

0 0 0 1.0000];

Wektor wartości początkowych zmiennych złączowych:

Q0 = [0 0 0 0 0 0]

Gotowa składnia funkcji realizującej kinematykę odwrotną. Dodatkowo jako jej parametry dodano 'verbose' wyświetlające ilość iteracji potrzebnych do odnalezienia punktu oraz 'pinv', które jest odpowiedzialne za sposób obliczania kinematyki odwrotnej.

wartosc_zlacz = staubli.ikine(porzadanepoloz, Q0, [1 1 1 1 1 1],'verbose','pinv');

Ten fragment kodu po prostu wyświetla na wykresie nową konfigurację manipulatora już po osiągnięciu pożądanego położenia.

staubli.plot(wartosc_zlacz);

Rysunek 8 Manipulator wychylony do pozycji określonej przez macierz „porzadanepoloz”

Dzięki odpowiedniemu dobraniu punktu pożądanego oraz zastosowaniu algorytmu danego parametrem „pinv” jego odnalezienie wymaga poniżej 18 iteracji funkcji ikine().

Własna trajektoria dzięki CTRAJ

Ostatnia realizowana przez nas funkcja to CTRAJ. Pozwala ona na wygenerowanie trajektorii, które w przestrzeni kartezjańskiej będą liniami prostymi. Nie przekłada się to oczywiście na linie proste w przestrzeni zmiennych złączowych.

Funkcja ctraj() jako parametry wejściowe przyjmuje położenie początkowe i końcowe efektora manipulatora oraz czas, czyli ilość kroków w jakim ma ten ruch zostać wykonany. Dla uproszczenia jako macierz położenia początkowego użyliśmy macierzy położenia pożądanego z kinematyki odwrotnej.

poczatek=[ 0.2752 -0.0000 0.9614 0.8482;

0.0000 1.0000 0.0000 0.0500;

-0.9614 0.0000 0.2752 0.6514;

0 0 0 1.0000]

Macierz homogeniczną położenia końcowego wygenerowaliśmy w analogiczny sposób co macierz pożądanego położenia dla kinematyki odwrotnej. Tutaj również pojawiał się problem z potrzebą uniknięcia konfiguracji osobliwej.

koniec=[-0.2063 0.4439 0.8720 0.4996;

0.0784 -0.8808 0.4669 0.7662;

0.9753 0.1647 0.1469 0.6727;

0 0 0 1.0000];

czas=1:100;

Ts= ctraj(poczatek, koniec,length(czas));

wektorpocz=[0 1.2 3.5 4 4 6]

zlacza=staubli.ikine(Ts,wektorpocz,'pinv');

Dodatkowo został dodany kod wyświetlający na wykresach wartości zmiennych złączowych, kąty YPR oraz wartości XYZ w przestrzeni kartezjańskiej. Nasz zespół dodatkowo dodał wykres przedstawiający ruch manipulatora realizujący trajektorię. Funkcja subplot() pozwoliła nam na wyświetlenie czterech wykresów na w jednym oknie.

subplot(2,2,1)

plot(czas,transl(Ts))

title('wartosci x,y,z')

subplot(2,2,2)

plot(czas,tr2rpy(Ts))

title('katy yaw, pitch, roll')

subplot(2,2,3)

plot(czas,zlacza)

title('wartosci zmiennych zlaczowych')

subplot(2,2,4)

staubli.plot(zlacza)

Rysunek 9 Subploty z opisanymi wyżej wykresami oraz okienko w którym animowany jest ruch manipulatora

Rysunek 10 Trajektoria ruchu końcówki manipulatora

Wnioski

1. Powodzenie zadania, którego celem lub etapem pośrednim jest modelowanie łańcucha kinematycznego robota zależy od poprawnego stworzenia tabeli DH. Błędy poczynione przy jej tworzeniu wykluczają poprawne wykonanie zadania.

2. Tabele DH należy tworzyć w ten sposób, aby przesunięcia między kolejnymi złączami były jak najmniejsze oraz zgodnie z innymi zasadami, zawartymi w książce prof. Jezierskiego „Podstawy Robotyki”. Bardzo pomocnym przy tworzeniu tej tabeli jest naszkicowanie kolejnych układów współrzędnych na rzucie izometrycznym łańcucha kinematycznego manipulatora

3. URDF jest bardzo wygodnym narzędziem do graficznego modelowania struktury kinematycznej manipulatora. Do jego głównych zalet zaliczyć należy możliwość pracy w chmurze na stronie mymodelrobot.appspot.com, oraz prosty do zrozumienia język w którym opisuje się elementy manipulatora. Do wad można według nas zaliczyć dosyć trudne ustawianie pożądanego widoku modelu oraz niewystarczającą liczbę tutoriali dostępnych w Internecie. Te dostępne wprowadzają nowych użytkowników jedynie w pewne podstawy, nie pokazując bardziej zaawansowanych rozwiązań.

4. MATLAB wraz z Robotic Toolbox'em autorstwa Pana Petera Corke'a, umożliwia pełne zamodelowanie łańcucha kinematycznego manipulatora. Jest to wszechstronne narzędzie znacznie upraszczające wszelkie obliczenia związane z rozwiązywaniem dla manipulatora zadania kinematyki prostej i odwrotnej. Dosyć poważnym problem jaki zauważyliśmy przy korzystaniu z tego narzędzia jest brak pełnej dokumentacji funkcji jakie udostępnia Toolbox, co znacznie utrudnia korzystanie z niego. Co więcej graficzna reprezentacja manipulatora jest słabej jakości i potrafi wprowadzić w błąd.

5. Zgodnie z tym co dowiedzieliśmy się semestr temu na wykładzie, kinematyka odwrotna stwarza o wiele więcej problemów niż kinematyka prosta, są to:

1. konieczność takiego dobrania zadań dla manipulatora by te znajdowały się w jego przestrzeni roboczej,

2. konieczność unikania konfiguracji osobliwych

6. Zmienne złączowe są zmiennymi o okresowo zmieniającej się wartości w związku z tym ich skorygowanie o 2π jest poprawne.

7. Przy tworzeniu drugiego modelu manipulatora w URDFie wprowadziliśmy ogniwa o zerowej długości, dzięki czemu mogliśmy umieścić dwa złącza w jednym punkcie przy zachowaniu struktury łańcucha kinematycznego.

8. W związku z opisem D-H, który wymusza „połamane” ustawienie manipulatora (-90 stopni w drugim złączu i +90 w trzecim złączu) zakresy obrotów w 2 i 3 złączu należało skorygować o 90 stopni.

Staubli TX90 http://www.upc.edu/sct/documents_equipament/d_181_id-492.pdf

Blog.gentle.pl http://blog.gentle.pl/pierwszy-robot-urdf/

Link do toolbox'a http://www.petercorke.com/Robotics_Toolbox.html

Str. 20-21

6

21

23

---