Traiettorie punto-punto, codice ed esperimenti

Sezione dedicata alla robotica mobile

Traiettorie punto-punto, codice ed esperimenti

Postby flz47655 » 09 Sep 2012, 17:04

Ciao a tutti
Volevo condividere con voi un programma C# ed un Firmware Arduino che ho scritto per calcolare la posizione in una traiettoria punto-punto imponendo velocità iniziale e finale zero.
Ho utilizzato il polinomio cubico a0 + a1*t + a2*(t^2) + a3*(t^3) che permette una variazione "dolce" della posizione e della velocità, essenziale per non avere bruschi movimenti che potrebbero danneggiare i motori e causare vibrazioni.

Il programma C# l'ho scritto per provare l'algoritmo e generare i dati da graficare in Excel, il firmware Arduino è stato testato con un paio di servo low-cost (HD-1800A ed MG995), i risultati sono fortemente influenzati dal fatto che i servo in oggetto hanno un loro sistema di controllo analogico basato su potenzimetro per cercare la posizione corretta. Nelle prove si passeranno i valori della posizione angolare che i servo dovranno raggiungere per passare da qo=0 a qf=180 gradi in un tempo tf=400ms.

Programma C#
Code: Select all
static void Traiettoria()
    {
      File.Delete("res.csv");
      // Utilizzo polinomio cubico
      // a0 + a1*t + a2*(t^2) + a3*(t^3)
      // per calcolare la posizione ed imporre velocità iniziale e finale = 0
      double tf = 400;           // tempo finale     
      double q0 = 0;             // posizione iniziale
      double qf = 180;           // posizione finale
      double qf0 = qf - q0;
      double tf2 = Math.Pow(tf, 2);
      double tf3 = Math.Pow(tf, 3);
      string csv = string.Empty;    // stringa per generare il file .csv

      for (double t = 1; t < tf + 1; t += 1)
      {               
        double t2 = Math.Pow(t, 2);
        double t3 = Math.Pow(t, 3);

        // Calcolo coefficienti eq. polinomiale       
        double a0 = q0;
        // a1 = 0, velocità iniziale = 0
        double a2 = (3 * qf0) / tf2;
        double a3 = (-2 * qf0) / tf3;

        double p = a0 + (a2 * t2) + (a3 * t3);    // posizione
        double v = (2 * a2 * t) + (3 * a3 * t2);  // velocità
        double a = (2 * a2) + (6 * a3 * t);       // accelerazione

        csv += p + ";" + v + ";" + a + ";" + "\r\n";
      }

      File.WriteAllText("res.csv", csv);   // scrivi il file con i risultati
      Process.Start("res.csv");             // apri il file (Excel)
    }

L'algoritmo viene valutato ogni unità di tempo, ecco i profili che si ottengono, grafici generati con Excel:

Posizione
Image

Velocità
Image

Accelerazione
Image

Come si può vedere l'unico problema potrebbe essere l'accelerazione, si potrebbe dire che il motore "parte in quinta" anche se nella realtà l'accelerazione sarà una rampa più o meno ripida date le caratteristiche fisiche del motore.
Se siete interessati ad una trattazione più matematica dell'algoritmo e dei suoi parametri fatemi sapere che pubblico qualche info aggiuntiva.

Firmware Arduino
Code: Select all
// Utilizzo polinomio cubico
// a0 + a1*t + a2*(t^2) + a3*(t^3)
// per calcolare la posizione ed imporre velocità iniziale e finale = 0     
#include <Servo.h>
Servo servo;

const float tf = 400.0f;       // tempo finale     
const float q0 = 0.0f;        // posizione iniziale
const float qf = 170.0f;       // posizione finale
const float qf0 = qf - q0;
const float tf2 = pow(tf, 2);
const float tf3 = pow(tf, 3);

unsigned long startTime, time;
void setup()
{
  servo.attach(9);  // servo sul pin9
  servo.write(0);   // vai a 0°
  delay(1000);      // aspetta il posizionamento
  startTime = millis();  // inizia a contare il tempo da ora
}

void loop()

  time = millis() -  startTime;
  servo.write(GetPosition(time));
}
 
float GetPosition(long t)
{
  if(t > tf)
  {            // Se siamo oltre il tempo finale
    return qf; // ritorna la posizione finale
  }
 
  // Calcola il quadrato e il cubo del tempo
  float t2 = pow(t, 2);
  float t3 = pow(t, 3);

  // Calcolo coefficienti eq. polinomiale
  float a0 = q0;
  // a1 = 0, velocità iniziale = 0
  float a2 = (3 * qf0) / tf2;
  float a3 = (-2 * qf0) / tf3;

  // Calcolo risultati
  float p = a0 + (a2 * t2) + (a3 * t3);        // posizione
  //double v = (2 * a2 * t) + (3 * a3 * t2);   // velocità
  //double a = (2 * a2) + (6 * a3 * t);        // accelerazione 
 
  return p;
}

Ho effettuato delle prove visive, in effetti si nota che la velocità segue il profilo prima graficato, e delle prove di consumo energetico con una resistenza Shunt di 1.2 ohm e 6V di tensione.
Nelle immagini 1 è il movimento del servo "grezzo" da 180 a 0°, mentre 2 è il movimento tramite la traiettoria punto-punto.
Si nota subito una maggiore facilità a trovare il punto di arrivo da parte del controllo del servo ed un consumo energetico di picco minore oltre a meno variazioni, col vantaggio di emettere meno rumore elettromagnetico.

Servo da 9g HD-1800A http://www.hobbytronics.co.uk/datasheets/HD-1800A.pdf
Image
Ho invertito i morsetti dell'oscilloscopio quindi il grafico risulta ribaltato, è chiaro però che torna quanto detto

Servo Standard MG995
Image
Questo servo è "famoso" per far fatica (senza carico) a trovare il punto di arrivo, con diverse oscillazioni attorno al punto finale. Il posizionamento tramite la traiettoria mostra che il servo trova con più facilità il punto di arrivo.

Concludendo, seppur la traiettoria punto punto implementata tramite equazione polinomiale sia maggiormente efficace su motori DC, anche tramite i servo è possibile sperimentare qualcosa e si può avere qualche vantaggio.

Se l'argomento è di interesse posso trasformare questo topic in un tutorial. Fatemi sapere i vostri commenti. Purtroppo non conosco Bascom ma la conversione del codice in questo linguaggio non dovrebbe presentare grossi problemi. Tutte le immagini sono caricate su imageshack, non occupano spazio sul server.

Ciao

UPDATE: Utilizzando servo.writeMicroseconds() al posto di servo.write() è possibile ottenere una risoluzione quasi 8 volte maggiore (0.09° invece di 0.7°) e di conseguenza movimenti meno scattosi.

UPDATE2:
Dispense su Arduino e Robotica pubblicate su Internet.zip
Per compensare Immagini cancellate da ImageShak
(195.54 KiB) Downloaded 267 times
Last edited by flz47655 on 11 Mar 2016, 12:41, edited 3 times in total.
flz47655
 
Posts: 639
Joined: 19 Jan 2012, 21:16

Re: Traiettorie punto-punto, codice ed esperimenti

Postby deluca » 11 Sep 2012, 18:00

ciao, complimenti per lo studio e il codice che hai sviluppato e ti invito a scrivere il tutorial.
NB. Ti invito anche ad imparare ed utilizzare il compilatore Bascom..... :D

di norma gli azionamenti meccanici che adottano motori DC o AC vengono controllati con profili di velocità di tipo trapezoidale.
un modo molto usato nella robotica per pianificare la traiettoria consiste nel generare un profilo di posizione lineare raccordato all’inizio e alla fine della traiettoria con tratti parabolici.
Il profilo di velocità che ne risulta ha il tipico andamento trapezoidale.

La traiettoria è quindi divisa in tre parti:
1.Accelerazione costante, velocità a rampa, posizione a parabola
2.Accelerazione nulla, velocità costante, posizione lineare
3.Decelerazione costante, velocità a rampa, posizione a parabola.

Però nella traiettoria con profilo di velocità trapezoidale l’accelerazione varierà in modo discontinuo.
Allora Il risultato è che il jerk ovvero la derivata dell’accelerazione assume valori infiniti.
Questo provoca grosse sollecitazioni al sistema meccanico, che portano a forti vibrazioni.
Per ovviare a questo problema, normalmente si modifica la pianificazione della traiettoria,
dando continuità al profilo di accelerazione, che assume andamento trapezoidale nelle due fasi
di accelerazione iniziale e decelerazione finale.

L'andamento della traiettoria è quindi divisa in tre parti principali:
1.Fase di accelerazione(l’accelerazione viene portata linearmente al valore massimo, mantenuta al valore massimo e riportata linearmente a zero);
2.Fase di crociera(velocità costante);
3.Fase di decelerazione(si decelera in modo speculare rispetto alla fase 1).

L'utilizzo del polinomio di ordine maggiore di 3 fino a 5 (traiettoria spline o multi punto) di norma non viene utilizzato nei controlli industriali.
Il più usato e preferito attualmente è sempre quello con profilo trapezoidale.

ciao
Ciao
Il mio sito: http://www.delucagiovanni.com ......e la chat: chat/
User avatar
deluca
Site Admin
 
Posts: 1104
Joined: 19 Jun 2011, 10:44
Location: 95123 - Catania (Italy)

Re: Traiettorie punto-punto, codice ed esperimenti

Postby -simone- » 17 Sep 2012, 18:30

Ciao complimenti per il lavorone!!! Le applicazioni potrebbero essere infinite, purtroppo, come ti ho accennato, sono agli inizi e per il momento non me la sento ancora di utilizzare il tuo codice per il mio progetto. Sai, sto ancora cercando di risolvere i problemi di controllo dei servi.... lo studierò e ti farò sapere - scuola permettendo- al più presto.

Ciao
-simone-
 
Posts: 2
Joined: 11 Sep 2012, 07:14

Re: Traiettorie punto-punto, codice ed esperimenti

Postby flz47655 » 26 Sep 2012, 11:42

Ciao e grazie a tutti,
Sono contento che l'argomento susciti interesse, non ho risposto subito perché volevo scrivere un tutorial ma visto che la stesura arriverà tra diverso tempo a causa di altri impegni per il momento volevo ringraziarvi.
Mi è venuta una domanda (anzi due): quindi se si volessero ottimizzare le vibrazioni basterebbe imporre delle condizioni anche allo snap (derivata del jerk)?
Supponendo quindi che imporre le derivate continue sia "benefico" una sorta di accelerazione sinusoidale avrebbe tutte le derivate continue e quindi sarebbe la soluzione teoricamente ottimale tralasciando la realtà dell'attuatore?
flz47655
 
Posts: 639
Joined: 19 Jan 2012, 21:16

Re: Traiettorie punto-punto, codice ed esperimenti

Postby flz47655 » 26 Sep 2012, 11:42

Ciao e grazie a tutti,
Sono contento che l'argomento susciti interesse, non ho risposto subito perché volevo scrivere un tutorial ma visto che la stesura arriverà tra diverso tempo a causa di altri impegni per il momento volevo ringraziarvi.
Mi è venuta una domanda (anzi due): quindi se si volessero ottimizzare le vibrazioni basterebbe imporre delle condizioni anche allo snap (derivata del jerk)?
Supponendo quindi che imporre le derivate continue sia "benefico" una sorta di accelerazione sinusoidale avrebbe tutte le derivate continue e quindi sarebbe la soluzione teoricamente ottimale tralasciando la realtà dell'attuatore?
flz47655
 
Posts: 639
Joined: 19 Jan 2012, 21:16


Return to Robotica

Who is online

Users browsing this forum: No registered users and 1 guest

cron