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

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

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

Casi particolari

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

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

Considerazioni sulla stima

Lo pseudocodice

Bibliografia

Like this:

Leave a Reply Cancel reply

rbonghi

Odometria

Casi particolari

Considerazioni sulla stima

Lo pseudocodice

Bibliografia

Share this:

Like this:

Leave a Reply Cancel reply