Odometria

La determinazione della posizione e dell’orientamento di un robot, utilizzando encoder incrementali montati su attuatori, che azionano e misurano le variazioni della loro rotazione, richiede che la posizione del robot sia riferita ad un sistema di coordinate definito [Bibliografia in fondo alla pagina].

Si considerano la velocità lineare v e quella angolare \omega costanti in ogni intervallo di campionamento, vedi la cinematica Uniciclo. Si assume quindi di conoscere la configurazione del robot \mathbf{q}\left(t_k\right) = \mathbf{q}_k all’istante di campionamento t_k, e i valori dei comandi di velocità v_k e \omega_k applicati all’intervallo \left[t_k,t_{k+1}\right). Partendo da una versione a tempo discreto del modello, utilizzando il metodo Runge-Kutta, si può approssimare ottenendo:

x_{k+1} = x_{k} + T \cdot v_k\cdot\cos \left(\theta_k + \frac{T\cdot\omega_k}{2}\right) \\ y_{k+1} = y_{k} + T \cdot v_k\cdot\sin\left(\theta_k + \frac{T\cdot\omega_k}{2}\right) \\ \theta_{k+1} = \theta_{k} + T \cdot \omega_k

che conduce all’espressione:

x_{k+1} = x_{k} + \frac{v_k}{\omega_k}\left(\sin\theta_{k+1}-\sin\theta_{k}\right) \\ y_{k+1} = y_{k} - \frac{v_k}{\omega_k}\left(\cos\theta_{k+1}-\cos\theta_{k}\right) \\ \theta_{k+1} = \theta_{k} + T \cdot \omega_k

tale approssimazione, rispetto alla discretizzazione con il metodo di Eulero permette di ridurre l’errore di approssimazione, ma il metodo di Runge-Kutta, tuttavia da risultati indefiniti anche quando \omega_k = 0, in questi casi ,quindi viene utilizzato il metodo di Eulero.

Odometry

Odometry

Sono tuttavia necessarie ulteriori modifiche per poter realizzare un algoritmo funzionale. Si potrebbe assumere quindi che gli ingressi v_k e \omega_k siano disponibili affinché sia possibile stimare le posizione finale del robot e che questi siano costanti durante tutto l’intervallo di campionamento. Tuttavia è risultato più conveniente ricostruire lo spazio percorso usando i sensori “propriocettivi”, destinati a misurare lo stato interno della piattaforma mobile, usando gli encoder e sapendo che

v_k\cdot T = \Delta_s \qquad \omega_k\cdot T = \Delta_\theta \qquad \frac{v_k}{\omega_k} = \frac{\Delta_s}{\Delta_\theta}

Nel caso della piattaforma mobile a trazione differenziale, avrà una opportuna modifica dei parametri, si definisce quindi:

\Delta_s = \frac{1}{2}\left(r_R\cdot\Delta_{\phi_R}+r_L\cdot\Delta_{\phi_L}\right) \Delta_\theta = \frac{1}{d}\left(r_R\cdot\Delta_{\phi_R}-r_L\cdot\Delta_{\phi_L}\right)

dove \Delta_{\phi_R} e \Delta_{\phi_L} sono le rotazioni rispettivamente della ruota destra e sinistra, r_R e r_L rispettivamente il raggio della ruota sinistra e destra delle ruote ed infine d è la distanza tra i centri delle ruote (interasse).

Da queste considerazioni ne risulta per la dinamica a tempo discreto l’espressione:

x_{k+1} = x_{k} + \frac{d}{2}\frac{\Delta_{\phi_R}+\Delta_{\phi_L}}{\Delta_{\phi_R}-\Delta_{\phi_L}}\left(\sin\theta_{k+1}-\sin\theta_{k}\right) \\ y_{k+1} = y_{k} - \frac{d}{2}\frac{\Delta_{\phi_R}+\Delta_{\phi_L}}{\Delta_{\phi_R}-\Delta_{\phi_L}}\left(\cos\theta_{k+1}-\cos\theta_{k}\right) \\ \theta_{k+1} = \theta_{k} + \frac{r}{d}\left(\Delta_{\phi_R}-\Delta_{\phi_L}\right)

Questa espressione ci indica come ottenere sulla base delle misure \Delta_{\phi_R} e \Delta_{\phi_L} tra l’intervallo t_k , t_{k+1} di poter ottenere quindi le informazioni di \theta_{k+1}, x_{k+1}, y_{k+1}.

Casi particolari

Come anticipato in precedenza, è importante analizzare alcuni casi in cui la formulazione non è valida.

Primo caso: \Delta_\theta = \Delta_{\phi_R}-\Delta_{\phi_L} \simeq 0

Il robot è avanzato linearmente e l’orientamento è rimasto costante, il metodo di Runge-Kutta perde quindi di validità e degenera nel metodo di Eulero. La nuova formulazione corretta sarà quindi:

x_{k+1} = x_{k} + \frac{r}{2}(\Delta_{\phi_R}+\Delta_{\phi_L})\cos\theta_k \\ y_{k+1} = y_{k} + \frac{r}{2}(\Delta_{\phi_R}+\Delta_{\phi_L})\sin\theta_k \\ \theta_{k+1} = \theta_{k}

Secondo caso: \Delta_s = \Delta_{\phi_R}+\Delta_{\phi_L} \simeq 0

Il robot sta ruotando sul proprio asse, quindi sta variando il proprio orientamento ma non le sue coordinate. La nuova formulazione quindi risulta, semplicemente:

x_{k+1} = x_{k} \\ y_{k+1} = y_{k} \\ \theta_{k+1} = \theta_{k} + \frac{r}{d}(\Delta_{\phi_R}-\Delta_{\phi_L})

Considerazioni sulla stima

Questo metodo, partendo da una configurazione iniziale del robot q_0 = (x_0,y_0,\theta_0) , basandosi sull’uso iterativo di queste formule fornisce una misura attendibile ma è soggetta ad una deriva che si accumula nel tempo e divine non trascurabile su percorsi lunghi.

Le varie fonti di errori possono essere:

  • slittamenti delle ruote
  • inaccuratezza nella calibrazione dei parametri cinematici (dimensioni ruote, dimensione interasse, etc etc)
  • errore introdotto nel metodo di integrazione.
  • approssimazione numerica

Lo pseudocodice

Dopo aver analizzato tutte le caratteristiche del sistema di localizzazione passiva, possiamo scrivere il codice associato:

#DEFINE RADIUS ...     //Definizione del raggio della ruota
#DEFINE WHEELBASE ...  //Definizione dell'interasse del robot
#DEFINE SPMIN ...      //Definizione di spostamento minimo
typedef struct coordinate {
   float x;
   float y;
   float theta;
} coordinate_t;
//Variabili aggiornate con lo spostamento effettuato dalle ruote robot
coordinate_t coord; //Variabile del vettore di configurazione iniziale del robot
int WheelSpR;      //Spostamento della ruota destra
int WheelSpL;      //Spostamento della ruota sinistra
...
coord.x = 0;
coord.y = 0;
coord.theta = 0;
float cosTh_old = cos(coord.theta);
float sinTh_old = sin(coord.theta);
...
//Codice
...
//Codice
void deadReckoning(void)
{
      //Salvataggio delle ultime coordinate calcolate
      volatile coordinate_t delta;
      //Calcolo dela somma degli spostamenti e la differenza degli
      //spostamenti della ruota sinistra e della ruota destra
      float SumSp = WheelSpR+WheelSpL;
      float DifSp = WheelSpR-WheelSpL;
   if(abs(DifSp) < SPMIN) {
      delta.theta = 0;
      delta.x = WheelSpR * cosTh_old;
      delta.y = WheelSpR * sinTh_old;
  } else if(abs(SumSp) < SPMIN) {
      delta.theta = DifSp / WHEELBASE;
      coord.theta = mod(coord.theta + delta.theta, 2*PI);
      sinTh_old = sin(coord.theta);
      cosTh_old = cos(coord.theta);
      delta.x = 0;
      delta.y = 0;
  } else {
      delta.theta = DifSp / WHEELBASE;
      coord.theta = mod(coord.theta + delta.theta, 2*PI);
      float cosTh_new = cos(coord.theta);
      float sinTh_new = sin(coord.theta);
      float radius = (WHEELBASE / 2) * (SumSp / DifSp);
      delta.x = radius * (sinTh_new - sinTh_old);
      delta.y = radius * (cosTh_old - cosTh_new);
      cosTh_old = cosTh_new;
      sinTh_old = sinTh_new;
  }
     //Vengono aggiornati i valori relativi alla posizione del robot
  coord.x += delta.x;
  coord.y += delta.y;
}

Bibliografia

  1. B. J. . E. H. R. and F. L., “”where am i?” sensor and methods for mobile robot position,” University of Michigan, p. 282, 1996.
  2. L. . V. L. . O. G. Siciliano, B. & Sciavicco, Robotics, Modelling, Planning and Control. Milano: Springer, 2009.

Copyright © 2013. All Rights Reserved.

0 Comments

Leave a Reply

This site uses Akismet to reduce spam. Learn how your comment data is processed.

%d bloggers like this: