View source for Navigation of the robot arm with ANN(Karin Vališová)

== Zadanie ==

Úlohou je natrénovať neurónovú sieť na ovládanie robotického ramena. Na vstupe dostáva pozíciu v priestore a ako výstup sieť vyprodukuje natočenie jednotlivých kĺbov, čiže vlastne rieši problém inverznej kinematiky.

Problém bol redukovaný na hľadanie mapovania z 2D do 3D priestoru. Vstupný priestor je rovina pred robotickým ramenom v ktorej sa pohybujú rameno, lakeť a zápästie, výstup je nastavenie ich uhlov.

Neskôr bola pridaná aj navigácia v tretej dimenzii, ktorá je ale prepočítavaná mimo siete. Zadané x,y,z súradnice sa prepočítajú do natočenia základne + pozície v 2D priestore (zrotovanej do základnej x,y roviny ako v trénovacej množine).

Projekt je rozdelený na dve časti - neurónova sieť a samotná navigácia.

== Hardware ==

===Robotické rameno===

Neurónová sieť je trénovaná pre parametre robotického ramena [http://www.lynxmotion.com/c-130-al5d.aspx AL5D od Lynxmotion]. Má 5 pohyblivých kĺbov:
#  základňa (otáča rameno okolo vlastnej osi)
#  rameno (pohybuje sa po x,y rovine a je schopná dosiahnuť uhol -90° až +90°)
#  lakeť (podobne ako rameno, ale os otáčania je obmedzená na -60° až +90°)
#  zápästie (podobne ako rameno)
#  otáčanie zápästia (otáča ruku okolo svojej osi)
#  ruka (je schopná uchopiť predmet)

[[Image:rameno.jpg]][http://www.lynxmotion.com/images/product/large/al5d-ns.jpg]

V projekte sa operuje s 2., 3. a 4. kĺbom, teda tými, ktoré natáčajú rameno v x,y osi. Neskôr bol pridaný aj pohyb základne a ruky, ktoré ale nie sú riadené neurónovou sieťou.

===Komunikácia sériovým portom===

Robotické rameno komunikuje cez sériový port. Keďže projekt je naprogramovaný v JAVA, tak boli použité [http://rxtx.qbang.org/wiki/index.php/Main_Page rxtx] knižnice. 

===Ovládanie ramena===

Na stránke výrobcu sa nachádza [http://www.lynxmotion.com/images/html/build136.htm dokumentácia protokolu komunikácie].

'''Príklad pohybu serva:'''

  #1 P1600 T1000 <cr>

Tento príkaz posunie servo 1 do pozície 1600 (pošle servu pulz so šírkou 1600 ms) za čas 1000 ms bez ohľadu na to, kde sa nachádza.

'''Príklad skupinového pohybu serva:'''

  #1 P1600 #2 P750 T2500 <cr>

Servo 1 a 2 sa súčasne pohybujú do vybratých pozícií za 2,5 sekundy.

Servám je možné posielať pulzy v rozsahu 600 - 2400, no existujú aj zakázané pozície pre vzájomné polohy serva.

Po naviazaní spojenia je nutné inicializovať rameno odoslaním pulzu so šírkou 1500 ms na všetky servá. Pohyb, ktorý následne rameno vykoná je veľmi prudký. Dôvodom je, že nezohľadnuje nijaké časové ani rýchlostné argumenty, pretože rameno nevie o počiatočnej pozícii na servách. 

  #0 P1500 #1 P1500 #2 P1500 #3 P1500 #4 P1500 <cr>

Pokyn pre inicializáciu.

== Neurónová sieť ==

===Architektúra===

Pre natrénovanie neuronovej siete bol použitý viacvrstvový perceptron. Celý kód bol napísaný vlastnoručne, optimalizáciou som sa dostala k sieti s dvoma skrytými vsrtvami (30 a 90 neurónov) a trénovacím faktorom alpha = 0.001 . Trénovaná je pomocou učenia so spätným šírením chyby (backpropagation learning) a na zrýchlenie konvergencie bolo použité momentum. Kompletný kód sa nachádza v prílohe. Aktivačné funkcie na skrytých vrstvách sú bipolárne sigmoidy, výstupná vrstva je lineárna.

Všetky nástroje na optimalizáciu neurónovej siete sú v kóde. 

===Trénovacie dáta===

Sieť má dva vstupné neuróny (pre x,y pozíciu v priestore pred ramenom) a tri výstupné (uhol pre servo ramena, lakťa a zápästia).
Výstup siete nie je šírka pulzu, ale uhol natočenia, ktoré sa potom prerátavajú. Dôvodom je, že dáta boli generované cez RIOS software, ktorý exportoval hodnoty v stupňoch v rozsahu jednotlivých kĺbov. 

   x                    y       rameno          lakeť            zápästie
   22.1219444270 12.1337833400	17.2000000000	-31.2357000000	-1.4157000000	
   21.6603393550	7.1602044110	-4.2000000000	0.9490000000	-49.7528000000	
   19.5597248080	11.3460903170	31.2000000000	-43.0828000000	-3.6405000000	
   10.6304016110	22.8893032070	45.2000000000	19.3121000000	-80.0899000000	

Vzorka trénovacej množiny. ''Pozn.: V skutočnej množine (data1.txt) sa nachádzajú aj z-súradnica a nastavenie všetkých ostatných kĺbov. Tieto dáta boli ale pri trénovaní ignorované''. 

Grafické znázornenie trénovacej množiny:

[[Image:Data_distr.jpg]]

V bode (0,0) sa nachádza stred základne robota. X-súradnice sa pohybujú v rozmedzí 5 - 37.5 cm, Y-súradnice 0 - 27 cm. Ako je vidieť na grafe, existuje množstvo polôh, do ktorých sa rameno nevie dostať.

Pre nepostačujúce výsledky musela byť niekoľkokrát zahusťovaná trénovacia množina. Záverečná hodnota bola 700 bodov v trénovacej množine a 100 v testovacej.

===Učenie===

Najnáročnejšia časť projektu bolo natrénovať sieť s čo najmenšou chybou. Konečné hodnoty priemernej odchylky na jeden kĺb pre sieť boli 1.764° na trénovacej množine 3.659° na testovacej množine.

Na grafe je znázornený vývoj absulotnej chyby pre všetky tri servá.

[[Image:Data_error.jpg]]

== Navigácia ==

Pre navigáciu ramena je použitá už uložená (najlepšia) inštancia neurónovej siete. Je ju možné prepísať vygenerovaním nového perceptrónu a použitím metódy void saveNet(), ktorá prepíše v pamati už existujúcu sieť.

Navigácia spočíva z krokov:
* Inicializácia perceptronu zo súboru

   static Perceptron p = Perceptron.loadFile();

* Prepočet 3D koordinátov na 2D a uhlu základne

   public static double[] getCoordinates(double x, double y, double z)
   {
        double[] coor = new double[2];
        double alpha = Math.acos(x/Math.sqrt(x*x + z*z));
        
        if(z<0)
        
            coor[0] = x*Math.cos(alpha) - z*Math.sin(alpha);
        else
            coor[0] = x*Math.cos(alpha) + z*Math.sin(alpha);
        
        
        coor[1] = y;
        return coor;
    }

* Zistenie uhlov zo siete

 public static double[] getAngles(double x, double y)
    {
        if(x>35 || x<5 || y>26 || y<0)
        {
        System.out.println("out of range");
        return null;
        }
        else
        {
        p.loadNumber(x,y);
        p.propagate();
        return p.out;
      }
    }

* Prepočítanie uhla na pulz a sformulovanie pokynu

   public static int toPulseShoulder(double deg)
    {
        int r = (int)(1500 + (10 * deg));
        if(r>2400) 
        {
            System.out.println("Pulse too long");
            r = 2400;
        }
        if(r<600) 
        {
            System.out.println("Pulse too short");
            r = 600;
        }
        return r;
    }
    
    public static String sendPulse(double[] out)
    {
        String result = "";
        result = "#1 P"+Convert.toPulseWirst(out[0]);
        result = result+" #2 P"+Convert.toPulseElbow(out[1]);
        result = result+" #3 P"+Convert.toPulseShoulder(out[2])+" T3000";
        return result;
    }

* Grab/put je pokyn, či má robot pustiť alebo uchopiť vec v bode (x,y)

  public static String grab()
    {
         return("#6 P900 T2000");
    }
    
    public static String put()
    {
         return("#6 P1600 T2000");
    } 
   
* Odoslanie pokynu do ramena a navrat do vychodzej polohy
  POZOR! Pred volaním funkcie main je nutné inicializovať rameno!
    
   public static void main(double x, double y, double z, boolean grab) throws IOException
    {
        ReadSerialPort a = new ReadSerialPort();
        a.main();
        a.print("#0 P"+Convert.toPulseBase(Navigation.getBaseAngle(x,y,z))+" T1000");
        a.print(Navigation.sendPulse(Navigation.getAngles(Navigation.getCoordinates(x,y,z))));
        try { Thread.sleep(5000);}
        catch(Exception e){;}
        if (grab) a.print(Navigation.grab());
        else      a.print(Navigation.put());
        try { Thread.sleep(3000);}
        catch(Exception e){;}
        
         a.print(Navigation.goHome());
    }

Používateľský interface

[[Image:GUI.jpg]]

<youtube>916zoUXUDzU</youtube>
Ukážka pohybu robotického ramena.

== Záver ==

Neurónová sieť bola aj napriek počiatočnym problémom schopná naučiť sa s pomerne vysokou presnoťou inverznú kinematiku. 

===Možné vylepšenia===

* Dosiahnutá odchylka sa dá ešte zmenšiť zahustením trénovacej množiny.

* Ošetriť vstupy do getAngles(double x, double y) aby sa nachádzali len v priestore dosiahnuteľnom ramenom (znázornené na grafe).

* Implementovať možnosť priblížiť sa do bodu pod určitým uhlom - v tejto variante ale treba nejak ošetriť jedinečnosť mapovania zo vstupu do výstupu.

== Zdrojový kód ==

[[Media: navigation1.jar]]