L’odometria parte 2

l’ultima volta avevamo trattato della discretizzazione del modello dell’uniciclo, trovando una nuova formula che potesse essere utile per la stima della posizione iniziale.

[tex]{x_{k+1} = x_{k} + frac{v_k}{omega_k}(sintheta_{k+1}-sintheta_{k})y_{k+1} = y_{k} – frac{v_k}{omega_k}(costheta_{k+1}-costheta_{k})theta_{k+1} = theta_{k} + T cdot omega_k[/tex]

Affinché sia possibile effettivamente formulare un codice che sia implementabile e sia funzionale, vanno effettuate nuovamente delle ulteriori modifiche.

Nell’ultima formula, perché sia possibile stimare le posizione finale del robot si è assunto che siano disponibili gli ingressi [tex]v_k[/tex] ed [tex]omega_k[/tex] e che questi siano costanti durante tutto l’intervallo di campionamento. Risulta più conveniente invece ricostruire lo spazio percorso usando i sensori propriocettivi disponibili (encoder) e sapendo che

[tex]v_kcdot T = Delta_s[/tex]

[tex]omega_kcdot T = Delta_theta[/tex]

[tex]frac{v_k}{omega_k} = frac{Delta_s}{Delta_theta}[/tex]

Il robot è costituito da una trazione differenziale e quindi lo spostamento totale del robot è dato da:

[tex]Delta_s = frac{r}{2}(Delta_{phi_R}+Delta_{phi_L})[/tex]

[tex]Delta_theta = frac{r}{d}(Delta_{phi_R}-Delta_{phi_L})[/tex]

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

Da queste considerazioni è quindi possibile scrivere nuovamente la formulazione originariamente esposta:

[tex]{x_{k+1} = x_{k} + frac{d}{2}frac{Delta_{phi_R}+Delta_{phi_L}}{Delta_{phi_R}-Delta_{phi_L}}(sintheta_{k+1}-sintheta_{k})y_{k+1} = y_{k} – frac{d}{2}frac{Delta_{phi_R}+Delta_{phi_L}}{Delta_{phi_R}-Delta_{phi_L}}(costheta_{k+1}-costheta_{k})theta_{k+1} = theta_{k} + frac{r}{d}(Delta_{phi_R}-Delta_{phi_L})[/tex]

Casi particolari

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

Primo caso: [tex]Delta_theta = Delta_{phi_R}-Delta_{phi_L} simeq 0[/tex]

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:

[tex]{x_{k+1} = x_{k} + frac{r}{2}(Delta_{phi_R}+Delta_{phi_L})costheta_ky_{k+1} = y_{k} + frac{r}{2}(Delta_{phi_R}+Delta_{phi_L})sintheta_ktheta_{k+1} = theta_{k}[/tex]

Secondo caso: [tex]Delta_s = Delta_{phi_R}+Delta_{phi_L} simeq 0[/tex]

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

[tex]{x_{k+1} = x_{k}y_{k+1} = y_{k}theta_{k+1} = theta_{k} + frac{r}{d}(Delta_{phi_R}-Delta_{phi_L})[/tex]

Considerazioni sulla stima

Questo metodo, partendo da una configurazione iniziale del robot [tex]q_0 = (x_0,y_0,theta_0) [/tex], 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 DIS ...  //Definizione dell'interasse del robot
#DEFINE SPMIN ...  //Definizione di spostamento minimo
//Variabile del vettore di configurazione iniziale del robot
//T = Orientamento, X ed Y
float vectorTXY[3] = {0,0,0};
//Variabili aggiornate con lo spostamento effettuato dalle ruote robot
int WheelSpR;        //Spostamento della ruota destra
int WheelSpL;        //Spostamento della ruota sinistra
int contr = 0;      //Uscita di controllo
...
//Codice
...
//Codice
void deadReckoning(void)
{
//Salvataggio delle ultime coordinate calcolate
float vectorRel[3] = {vectorTXY[0],vectorTXY[1],vectorTXY[2]};
float SumSp = 0;
float DifSp = 0;
//Calcolo dela somma degli spostamenti e la differenza degli spostamenti
//della ruota sinistra e della ruota destra
SumSp = WheelSpR+WheelSpL;
DifSp = WheelSpR-WheelSpL;
if(fabsf(DifSp) < SPMIN)
{
vectorTXY[1] = vectorRel[1] + (RADIUS/2)*SumSp*cosf(vectorRel[0]);
vectorTXY[2] = vectorRel[2] + (RADIUS/2)*SumSp*sinf(vectorRel[0]);
}
else if(fabsf(SumSp) < SPMIN)
{
vectorTXY[0] = vectorRel[0] + (RADIUS/DIS)*DifSp;
}
else
{
vectorTXY[0]=vectorRel[0]+(RADIUS/DIS)*DifSp;
vectorTXY[1]=vectorRel[1]-(DIS/2)*(SumSp/DifSp)*(sinf(vectorTXY[0])-sinf(vectorRel[0]));
vectorTXY[2]=vectorRel[2]+(DIS/2)*(SumSp/DifSp)*(cosf(vectorTXY[0])-cosf(vectorRel[0]));
}
//Viene normalizzata la grandezza dell'angolo Theta tra 0 e 2*pi
vectorTXY[0] = fmodf(vectorTXY[0], 2*PI);
}

Copyright © 2009. All Rights Reserved.

Leave a Reply