Controllo PID con TurtleSim

Sommario

Move Turtle è un semplice script Python per ROS che muove la tartaruga in TurtleSim di ROS verso coordinate specificate (punti). Questo progetto è iniziato come progetto di corso per CSCI473 al Colorado School of Mines (CSOM) e in seguito si è evoluto in un progetto che ho usato per aiutarmi a imparare come possono essere creati i sistemi a ciclo chiuso, conoscenze che ho poi utilizzato nel mio lavoro al laboratorio Human Centered Robotics (HCR) del CSOM.

Contesto

Per un po’ di contesto, ROS sta per Sistemi Operativi per Robot. ROS è un middleware robotico open-source, solitamente abbinato al sistema operativo Ubuntu, che viene utilizzato per gestire meglio i componenti/clusters software di un sistema robotico. Puoi imparare di più su come usare ROS tramite il wiki ufficiale di ROS. Il codice che scrivi in ROS è o in C++ o in Python.

TurtleSim è un semplice simulatore robotico 2D usato come introduzione a ROS e ai pacchetti ROS per i nuovi utenti di ROS. Tutto ciò che TurtleSim fa è aprire una semplice visualizzazione 2D con una tartaruga. Quella tartaruga agisce come il “robot” e puoi inviare messaggi a quel “robot” per muoverlo e/o farlo girare. Muovendosi, TurtleSim genera comunque un certo errore casuale per simulare in modo semplice la fisica del mondo reale.

Sapendo cosa sono ROS e TurtleSim, perché li stavo usando? Beh, nella primavera del 2020 stavo seguendo un corso chiamato “Human Centered Robotics” al Colorado School of Mines come parte del mio carico di studi in informatica. Il corso era tenuto dal Dr. Hao Zhang. Il corso consisteva in tre progetti e il primo progetto prevedeva l’installazione di ROS, l’apprendimento di come usare ROS e l’implementazione di un algoritmo a ciclo aperto o a ciclo chiuso che avrebbe fatto disegnare alla tartaruga in TurtleSim una M. La descrizione originale del progetto può essere vista QUI.

Quando seguivo il corso, ho completato questo progetto implementando un algoritmo a ciclo aperto. Questa soluzione funzionava ma era davvero scadente e disegnava una A che era “abbastanza buona” ma lontana dall’essere “ottima”. Dopo aver inviato il progetto intorno alla metà di febbraio 2020, me ne sono dimenticato fino a circa un anno dopo, intorno a febbraio 2021.

Sfida

A febbraio 2021 sono stato assunto come Assistente di Ricerca presso l’Human Centered Robotics (HCR) Lab al Colorado School of Mines dove ho lavorato sotto la supervisione del Dr. Hao Zhang. L’obiettivo del laboratorio è fare ricerca su “autonomia collaborativa continua, con l’obiettivo di permettere ai robot di operare e adattarsi per lunghi periodi di tempo”. A partire dal 01-09-2022 (1 settembre 2022), il laboratorio si è trasferito dal Colorado School of Mines all’Università del Massachusetts Amherst. Ma, quando ho lavorato lì, erano ancora situati in Colorado.

All’HCR Lab, mi è stato assegnato il compito di lavorare sul progetto Triton. Il progetto Triton consisteva di molteplici, dozzine, di robot chiamati Triton. I Triton erano robot terrestri triangolari con ruote omni che avevano questo aspetto:

Insieme ai Triton, otto Telecamere IR di OptiTrack erano disposte in un “ordine ottagonale” a pochi piedi dal suolo. Utilizzando quelle telecamere, il software Motive di OptiTrack, un PC con Windows 10 e ROS; viene creato uno spazio di 2 metri per 2 metri per 2 metri (x, y, z) dove è possibile determinare la posizione esatta nel mondo reale di oggetti con marker di motion capture.

Il mio primo compito nel progetto Triton era creare script che facessero muovere il robot Triton verso una specifica posizione nel mondo reale. All’epoca non ero sicuro di come ottenere questo risultato, ma ricordavo il primo progetto di CSCI473 dove il concetto di un sistema a ciclo chiuso mi era stato introdotto, quindi ho iniziato a fare ricerche approfondite sui sistemi a ciclo chiuso e, a sua volta, sui sistemi di retroazione.

Ricerca

Durante la mia ricerca, ho scoperto questo ottimo video di AerospaceControlsLab:

In questo video, l’idea di un sistema PID mi è stata mostrata e spiegata. Un sistema PID è un sistema a ciclo chiuso che produce specifici output basandosi su dati dal mondo reale. In termini base, è un sistema di retroazione che aggiusta il suo output in base alla differenza tra il valore desiderato e il valore misurato. Questa è la formula per un controllore PID:

$$ u(t) = K_p e(t) + K_i \int e(t) dt + K_d \frac{de(t)}{dt} $$

$u(t)$ è il segnale di controllo.
$K_p$, $K_i$, $K_d$ sono rispettivamente i guadagni proporzionale, integrale e derivativo.
$e(t)$ è il segnale di errore (la differenza tra l’output desiderato e quello reale).
$\int e(t) dt$ è l’integrale dell’errore nel tempo.
$\frac{de(t)}{dt}$ è la derivata dell’errore.

Per approfondire, guarda il video di AerospaceControlsLab o questa fantastica voce di Wikipedia da cui proviene la formula.

Dopo aver fatto le mie ricerche e aver ricevuto un po’ di aiuto dai miei amici e professori brillanti, ho determinato che utilizzare un Controllore Proporzionale, Controllore P, sarebbe stato il migliore per aiutare il Triton a raggiungere una coordinata specifica.

Poiché il Triton è un robot terrestre, devo preoccuparmi solo delle coordinate X e Y. Sapendo questo, ho poi dovuto determinare quali valori desiderati e valori misurati rilevare e processare all’interno del/i Controllore(i) P. Nella mia ricerca ho trovato questo ottimo post sul wiki di ROS chiamato Go to Goal dove si concentravano sui seguenti errori:

Errore di distanza: La differenza nella distanza tra la posizione desiderata (X, Y) e la posizione corrente (X, Y).
Errore di theta: La differenza tra l’orientamento desiderato e l’orientamento corrente.

Soluzione Proposta

Conoscendo tutto ciò, la soluzione per far muovere il Triton verso una specifica coordinata reale è utilizzare un Controllore Proporzionale per l’errore di distanza del Triton e per l’errore di theta rispetto alla coordinata obiettivo e alla coordinata corrente del Triton. Ma prima di implementare questo in un sistema fisico, volevo testare l’idea in simulazione e, all’epoca, pensavo che usare TurtleSim come simulazione fosse la scelta migliore.

Test in simulazione

Con la soluzione teorica delineata, ho scelto inizialmente di convalidarla tramite simulazione, piuttosto che in uno scenario reale. Il mondo reale è spesso pieno di rumore che potrebbe offuscare l’efficacia della soluzione e portare a gestire problemi irrilevanti. Riconoscendo questo, ho rivisitato il mio progetto di CSCI473 e l’ho adattato come banco di prova per questa soluzione teorica.

Dopo aver aggiornato il vecchio codice da ROS Melodic a ROS Noetic e con qualche raffinamento iterativo, sono riuscito a far funzionare la soluzione proposta in TurtleSim. Dimostrazioni della sua funzionalità, con alcuni bei percorsi di coordinate obiettivo, possono essere trovate alla fine di questo post del blog. Il codice per tutto questo può essere visualizzato QUI.

Conclusione

In sintesi, attraverso questo progetto, la tartaruga di TurtleSim naviga efficacemente verso coordinate impostate utilizzando il sistema a ciclo chiuso, superando significativamente il mio iniziale sistema a ciclo aperto del 2020. Dopo averlo testato con successo, ho implementato questa soluzione con i robot Triton. Ma il rumore del mondo reale ha introdotto problemi inaspettati che hanno richiesto settimane di debug e test nel mondo reale. Dopo quelle settimane, alla fine sono riuscito a far raggiungere ai Triton coordinate reali specifiche utilizzando la soluzione che avevo testato in TurtleSim. Ecco una demo del Triton che si muove verso coordinate reali specifiche usando il metodo testato in TurtleSim:

Riflettendoci, avrei voluto usare una simulazione che simulasse meglio la fisica del mondo reale. TurtleSim è ottimo per imparare ma non è ideale per testare idee di robotica nel mondo reale. All’epoca pensavo che sarebbe stato sufficiente per il mio semplice caso d’uso, ma in seguito si è dimostrato non essere così.

L’obiettivo principale di questo post del blog era delineare la creazione di un sistema a ciclo chiuso che consentisse alla tartaruga in TurtleSim, e successivamente ai robot Triton, di navigare con precisione verso posizioni obiettivo specificate (X, Y). Credo che questo obiettivo sia stato raggiunto, e potrebbe aver suscitato qualche interesse nel mio lavoro presso l’HCR Lab. Sapendo questo, ho in programma di scrivere altri post sul blog riguardo la mia esperienza di lavoro all’HCR Lab…

Demo di Moving Turtle (TurtleSim)

Questo video contiene le seguenti demo:

Nodale di Cayley: Un semplice disegno [Nodale di Cayley]
Cerchi: Un disegno simile a una ragnatela
Cerchi Interni: Un disegno di “cerchi dentro cerchi”
M: Un disegno a M molto semplice