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 e quella angolare
costanti in ogni intervallo di campionamento, vedi la cinematica Uniciclo. Si assume quindi di conoscere la configurazione del robot
all’istante di campionamento
, e i valori dei comandi di velocità
e
applicati all’intervallo
. Partendo da una versione a tempo discreto del modello, utilizzando il metodo Runge-Kutta, si può approssimare ottenendo:
che conduce all’espressione:
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 , in questi casi ,quindi viene utilizzato il metodo di Eulero.
Sono tuttavia necessarie ulteriori modifiche per poter realizzare un algoritmo funzionale. Si potrebbe assumere quindi che gli ingressi e
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
Nel caso della piattaforma mobile a trazione differenziale, avrà una opportuna modifica dei parametri, si definisce quindi:
dove e
sono le rotazioni rispettivamente della ruota destra e sinistra,
e
rispettivamente il raggio della ruota sinistra e destra delle ruote ed infine
è la distanza tra i centri delle ruote (interasse).
Da queste considerazioni ne risulta per la dinamica a tempo discreto l’espressione:
Questa espressione ci indica come ottenere sulla base delle misure e
tra l’intervallo
di poter ottenere quindi le informazioni di
.
Casi particolari
Come anticipato in precedenza, è importante analizzare alcuni casi in cui la formulazione non è valida.
Primo caso: 
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:
Secondo caso: 
Il robot sta ruotando sul proprio asse, quindi sta variando il proprio orientamento ma non le sue coordinate. La nuova formulazione quindi risulta, semplicemente:
Considerazioni sulla stima
Questo metodo, partendo da una configurazione iniziale del robot , 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.
Copyright © 2013. All Rights Reserved.
0 Comments