Università degli Studi di Catania Corso di Laurea in Ingegneria dell’Automazione e Controllo dei Sistemi Complessi
Biorobotica Anno 2009/2010
Prof. Ing. Arena Paolo
Di Mauro Gianluca
Ing. Patanè Luca
Patti Giuseppe
Ing. Alessandra Vitanza
Introduzione
Il nostro lavoro si è focalizzato sullo studio e la realizzazione di un controllore in coppia per l’introduzione di tale tipologia di controllo nei motori del Dynamic Robotic Simulator. A tal fine si è proceduto, grazie all’utilizzo di strumenti software quali ODE (Open Dynamic Engine) e Visual Studio, allo sviluppo del motore in un ambiente ad hoc, e successivamente, alla sua integrazione nel simulatore vero e proprio.
Open Dynamic Engine (ODE)
Open Dynamic Engine (ODE) è una libreria distribuita sotto licenza BSD e LGPL, adatta alla simulazione di strutture di corpi rigidi articolati, cioè di corpi aventi varie forme che sono connessi insieme da giunti. Le sue principali peculiarità sono: • I corpi rigidi possono avere distribuzione arbitraria della massa. • Tipi di giunti: sferico, cardine, lineare, cardine-2, fisso, universale, motore angolare, motore lineare. • Primitive di collisione: sfera, parallelepipedo, cilindro, capsula, piano, raggio, mesh convessa. • Spazi di collisione: Quad tree, hash, semplice. • Metodo di simulazione: Le equazioni del moto sono derivate da un moltiplicatore di Lagrange basato sulla velocità Trinkle/Stewart e Anitescu/Potra. • Viene impiegato un integratore del primo ordine. Esso è rapido ma non abbastanza accurato per valutazioni quantitative. Ordini maggiori sono previsti in futuro.
• Possibilità di scelta del metodo di avanzamento: metodo “big matrix” o metodo iterativo QuickStep. • Modello di contatto e frizione: Basato sul risolutore LCP descritto da Baraff. ODE implementa un'approssimazione del modello di frizione di Coulomb più rapida. • Possiede un'interfaccia nativa scritta in C (anche se ODE e scritto in C++) • Possiede un'interfaccia C++ costruita sull'interfaccia C. • Ha molti unit test e molti altri ne vengono continuamente sviluppati. • Possiede delle ottimizzazioni specifiche per le varie piattaforme. • Possibilità di essere compilato sia in singola che in doppia precisione. Compilando in singola precisione si avrà una maggiore velocità abbinata ad un minor utilizzo di memoria ma si avrà un numero di errori di simulazione maggiore.
Tipi di dati , primitive e oggetti
I tipi di dati gestiti da ODE sono: • dReal float o double a seconda della precisione utilizzata Il formato usato dal compilatore. Per l'architettura Intel x86 e il IEEE 754 • dVector3 dReal[4] [x, y, z, 0.0] • dVector4 dReal[4] [x, y, z, 1.0] • dQuaternion dReal[4] [w, x, y, z] • dMatrix3 dReal[3*4] Matrice 3x3 memorizzata per righe (ogni riga ha 4 elementi) • dMatrix4 dReal[4*4] Matrice 4x4 memorizzata per righe Mentre, gli oggetti e le primitive gestite da ODE che abbiamo utilizzato nella nostra simulazione sono:
• dWorld - Rappresenta l'universo della simulazione. Esso contiene i parametri fondamentali della simulazione. • dSpace - Rappresenta un corpo rigido. • dBody - Rappresenta una forma geometrica utilizzata nella valutazione delle collisioni. • dGeom - Rappresenta uno spazio di collisione, ovvero un raggruppamento di dGeom. • dJoint - Rappresenta un giunto tra due corpi rigidi. • dJointGroup - Rappresenta un insieme di giunti. Scendendo in dettaglio, si analizzano alcune delle funzioni disponibili, tra cui: dWorldID dWorldCreate(); void dWorldDestroy (dWorldID);
Questa coppia di funzioni crea e distrugge un oggetto dWorld. Nonostante si possano creare più dWorld, all'interno di una simulazione solitamente ne viene creato uno solo. void dWorldSetGravity (dWorldID, dReal x, dReal y, dReal z); void dWorldSetERP (dWorldID, dReal erp); void dWorldSetCFM (dWorldID, dReal cfm);
Queste tre funzioni impostano rispettivamente la forza di gravità, il parametro ERP ed il parametro CFM 1, spiegati nel paragrafo successivo, per il dato dWorld. void dWorldStep (dWorldID, dReal stepsize); void dWorldQuickStep (dWorldID, dReal stepsize);
Queste funzioni realizzano un avanzamento nella simulazione di Step size secondi. I due metodi realizzano due differenti approcci alla soluzione della matrice dei vincoli. Il primo metodo è più lento (complessità O( ) rispetto a O(n⋅N ) dove N è il numero di iterazioni, [1] Vedi paragrafo ERP e CFM
parametro impostabile ed n è il numero di righe della matrice dei vincoli) ed utilizza più memoria ( O( ) rispetto a O(n)) rispetto al secondo, ma risulta essere più accurato. In ogni caso i due metodi sono intercambiabili. dBodyID dBodyCreate (dWorldID); void dBodyDestroy (dBodyID);
Provvedono a creare e distruggere un corpo rigido.
void void void void void
dBodySetPosition (dBodyID, dReal x, dReal y, dReal z); dBodySetRotation (dBodyID, const dMatrix3 R); dBodySetQuaternion (dBodyID, const dQuaternion q); dBodySetLinearVel (dBodyID, dReal x, dReal y, dReal z); dBodySetAngularVel (dBodyID, dReal x, dReal y, dReal z);
Queste funzioni impostano la posizione, la rotazione (attraverso matrice di rotazione o attraverso quaternione), la velocità lineare e la velocità angolare. Esistono anche le funzioni per rilevare suddetti valori. Durante il corso della simulazione è sconsigliato impostare direttamente velocità e posizione dei corpi rigidi poiché questo potrebbe portare all'instabilità della simulazione stessa. Piuttosto bisognerebbe agire attraverso l'applicazione di forze. void void void void
dBodySetMass (dBodyID, const dMass *mass); dBodyGetMass (dBodyID, dMass *mass); dBodyAddForce(dBodyID, dReal fx, dReal fy, dReal fz); dBodyAddTorque(dBodyID, dReal fx, dReal fy, dReal fz);
Queste funzioni riguardano l'aspetto dinamico del corpo rigido, impostano la massa del corpo e agiscono su di esso attraverso l'applicazione di forze vettoriali o coppie di forze per generare un momento di rotazione. Una limitazione di ODE non permette che il centro di massa di un corpo abbia coordinate diverse dall'origine del sistema di riferimento dell'oggetto. In poche parole centro geometrico e centro di massa devono coincidere. Un dGeom rappresenta una generalizzazione che raggruppa tutte le
possibili entità geometriche presenti in ODE. Le principali operazioni applicabili a un dGeom sono: void dGeomDestroy (dGeomID); void dGeomSetBody (dGeomID, dBodyID); dBodyID dGeomGetBody (dGeomID); void dGeomSetPosition (dGeomID, dReal x, dReal y, dReal z); void dGeomSetRotation (dGeomID, const dMatrix3 R); void dGeomSetQuaternion (dGeomID, const dQuaternion);
La prima consente di eliminare un dGeom. La seconda e la terza gestiscono il collegamento che gli oggetti geometrici hanno con i corpi rigidi. Un dGeom collegato a un dBody vedrà la propria posizione (e orientamento) aggiornata assieme a quella del dBody. Utilizzare la funzione dGeomSetBody() con argomento 0 equivale a rendere indipendente il dGeom che cosi avrà una posizione e orientazione propri. Questo è utile per creare oggetti statici (come pareti o pavimenti) che non risentono della presenza degli altri oggetti (non possiedono massa o inerzia) ma che reagiscono alle collisioni. Quando un dGeom viene collegato ad un dBody il suo vettore posizione e il suo quaternione di orientamento vengono eliminati e sostituiti da delle grandezze spiazzamento. Queste grandezze spiazzamento sono riferite rispetto al sistema di riferimento del corpo rigido al quale il dGeom è collegato. Queste grandezze si possono impostare attraverso le funzioni. Un dBody può avere associati più dGeom mentre un dGeom può essere associato ad un solo dBody. Questo permette di avere dei corpi rigidi di forma complessa la quale risulta essere composta dai vari dGeom ad esso associati. int dGeomIsSpace (dGeomID);
Un dGeom è anche una generalizzazione di un dSpace. Questo consente di formare una gerarchia ad albero in cui vari dGeom vengono raggruppati in un dSpace il quale può diventare parte di un altro dSpace.
Questo consente di ottimizzare il codice di collision detecting e di gestire separatamente gli spazi di collisione per determinati gruppi di oggetti. E conveniente infatti raggruppare gli oggetti in spazi di collisione poiché se, ad esempio, si sa a priori che determinati oggetti non entreranno mai in collisione tra loro, li si può raggruppare in spazi di collisione differenti ed evitare di far collidere i due spazi. Erp e CFM 2 Quando un giunto collega due corpi, le posizioni e gli orientamenti dei corpi devono assumere le posizioni e orientamenti relativi richiesti. Tuttavia, è possibile che i corpi vengano a trovarsi in posizioni in cui i vincoli non sono soddisfatti. Quest’errore può avvenire in due modi: 1. Se l'utente imposta la posizione / orientamento di un corpo senza impostare correttamente la posizione / orientamento dell’altro corpo. 2. Durante la simulazione , facendo si che i corpi si vengano a trovare lontani dalle posizioni richieste.
Per ridurre l’errore al giunto: Durante ogni step di simulazione ad ogni giunto viene applicata una forza speciale per riportare i corpi al corretto allineamento. Questa forza è controllata dall’ error reduction parameter (ERP) che ha un valore tra 0 e 1. Impostando l’ ERP si specifica che proporzione di errore di giunto sarà fissato durante il prossimo step di simulazione(entità delle forza necessaria al corretto allineamento). Se l’errore ERP=0 nessuna forza di 2
Vedi Documentazione ODE- http://www.ode.org/ode-latest-userguide.html 3.6 Joint Groups
correzione è applicata ai giunti , se ERP=1 la simulazione provvederà a correggere l’errore (si consiglia un errore tra 0.1 e 0.8,il default è 0.2). Nel simulatore è possibile fissare un valore di ERP globale che ha effetto su tutti i giunti dell’ambiente,ed è possibile assegnare inoltre valori di ERP singolarmente per i corpi presenti nell’ambiente. Soft constraint and constraint force mixing (CFM)
La maggior parte dei vincoli sono di natura "hard". Ciò significa che i vincoli rappresentano condizioni che non sono mai violate. In pratica i vincoli possono essere violati con l'introduzione involontaria di errori nel sistema, ma il parametro di riduzione di errore può essere impostato per correggere questi errori. Non tutti i vincoli sono rigidi,però alcuni "soft" sono dei vincoli progettati per essere violati. Ad esempio, il vincolo che impedisce il contatto cioè la collisione di oggetti e la penetrazione è un vincolo “hard” di default, quindi si comporta come se le superfici di collisione siano in acciaio. Ma può essere trasformato in un vincolo morbido per simulare materiali morbidi,consentendo in tal modo una certa penetrazione naturale dei due oggetti quando essi sono vincolati insieme. Ci sono due parametri che fissano i vincoli tra hard e soft. Il primo è (ERP)il secondo (CFM) .
Impostare ERP e CFM
ERP e CFM può essere impostato in modo indipendente in molti giunti. Possono essere impostati in giunti di contatto, in termini comuni e vari altri luoghi, per controllare la “morbidezza” e l’ ”elasticità” del giunto (o del limite comune). Se il CFM è impostato a zero, il vincolo sarà hard. Se il CFM è impostato ad un valore positivo, sarà possibile violare il vincolo (ad esempio, per i vincoli di contatto, forzando i due oggetti ad entrare in contatto tra loro). In altre parole, il vincolo sarà morbido, e la morbidezza aumenta con l'aumentare di CFM. Che cosa realmente sta
accadendo qui è che il vincolo può essere violato di una quantità proporzionale a CFM volte la forza necessaria a far rispettare il vincolo. Si noti che un CFM negativo può generare instabilità. Regolando i valori di ERP e CFM, è possibile ottenere vari effetti. Per esempio è possibile simulare vincoli elastici, dove i due corpi oscillano come se collegati da molle. Oppure si possono simulare vincoli più smorzati, senza oscillazioni. Se per il sistema, si vogliono impostare i valori di kp (costante elastica) e dello smorzamento costante kd, bisogna impostare:
dove h è lo step di campionamento. Questi valori daranno lo stesso effetto di un sistema molla-smorzatore. Aumentare la CFM, in particolare il CFM globale,permette di ridurre gli errori nella simulazione numerica. Se il sistema si trova nei pressi di una singolarità, aumentare il valore di CFM può aumentare notevolmente la stabilità. Valori di ERP e CFM all’interno della simulazione. Per settare tali valori all’interno della simulazione, si può agire usando i tag specifici all’interno del file:
File simfile.sim.xml All’interno del tag Simulation sono impostati i valori di ERP e CFM globali per l’intera simulazione : <Simulator> <Simulation name = "Simulazione"> <erp value="0.077"/> <cfm value="0.000001"/>
Inoltre, sono stati impostati diversi valori di ERP e CFM per alcuni oggetti presenti nella simulazione: Tribotp.dae <technique profile="ODE"> <soft>true</soft> <softcfm>0.00001</softcfm> <softerp>0.6</softerp>
ObstacleDynp.dae <technique profile="ODE"> <soft>true</soft> <softcfm>0.00001</softcfm> <softerp>0.6</softerp> </technique>
Il controllo
Per quanto riguarda il controllo del motore si è scelto, considerando il fatto che nella realtà questa tipologia di motori verrà controllata da un PID, di sintetizzare un controllore di siffatta specie.
Il PID Lo schema generale di un controllore di tipo PID è il seguente:
In pratica lâ&#x20AC;&#x2122;uscita di un controllore PID è costituita dalla somma di tre termini:
dove Kp,, Ti, Td sono tre costanti positive.
Sintesi del PID
La sintesi del PID, ovvero la determinazione dei guadagni Kp, Kd, Ki, è stata effettuata attraverso 2 metodologie implementative :la oramai nota procedura analitica, quella empirica.
Modello matematico del motore Al fine di determinare i tre parametri del controllore PID (Kp,Kd , Ki) basandoci su specifiche riguardanti la risposta al gradino si è calcolato, a partire dal servo motore Dynamixel AX12+ 2, il seguente modello matematico.
Con P(t) posizione angolare in rad ω(t) velocità nagolare rad/sec τ(t) momento meccanico Nm J(t) momento angolare Nms I momento d’inerzia
Lo schema finale, quello cioè che verrà utilizzato nel nostro lavoro, considerando il plant del secondo trovato precedentemente è
2
Per maggiori informazioni consultare l’appendice D Dynamixel AX12+
Tale modello permette di mettere in evidenza come il controllo da noi implementato, non sia un vero e proprio controllo di coppia come quelli già visti in letteratura. Infatti, come si può notare l’uscita non è (posizione attuale rappresentata da una coppia ma da un angolo angolare del giunto), ciò è dovuto al fatto che ODE maschera il vero controllo di coppia attraverso la funzione dBodyAddTorque. Quindi alla luce di ciò, imponendo il riferimento ( posizione angolare desiderata del giunto) , il PID calcolerà il valore di coppia da passare alla funzione sopracitata. Metodo analitico : plant del secondo ordine Si vuole configurare un controllore PID per un sistema rappresentato da una equazione differenziale del secondo ordine basandosi sui requisiti riguardanti la risposta al gradino quali tempo di salita (tr) e di assestamento(ts). Consideriamo la funzione di trasferimento nel dominio di Laplace di un plant del secondo ordine:
Successivamente consideriamo la funzione di trasferimento di un controllore PID di ordine intero:
Aggiungiamo il controllore al sistema e chiudiamo il loop ottenendo quanto raffigurato nel seguente diagramma a blocchi:
La funzione di trasferimento a ciclo aperto del sistema assume la forma:
Mentre quella a ciclo chiuso diventa:
Quindi dopo le opportune semplificazioni e rappresentando il sistema in forma di Bode si ha:
Osservando la funzione di trasferimento a ciclo chiuso, si può notare la presenza di tre poli. Imponiamo che un polo si trovi molto lontano dall’asse immaginario ad una distanza 5ξωn, mentre i due poli rimanenti
siano dominanti. Quanto detto viene illustrato nel seguente diagramma:
Si è dunque considerato che la dinamica del sistema dipende solamente dai due poli dominanti. Riscrivendo l’equazione del sistema al denominatore ed eguagliandola ad una funzione di trasferimento con due poli dominanti e uno distante dall’asse immaginario si ha:
Eguagliando i termini dello stesso grado si ottiene:
Risolvendo dunque rispetto ai parametri del PID:
Si sono così ricavati i tre guadagni del controllore in funzione di ξ e ωn. Il prossimo passo consiste nel determinare ξ e ωn. Fatto ciò i parametri Kp, Kd e Ki sono univocamente determinati. Introduciamo quindi le specifiche sulla risposta temporale necessarie per configurare il PID e che imponiamo dall’esterno: 1. il tempo di assestamento (settling time), secondo il criterio del 2%:
2. Il tempo di salita (rise time):
Considerando il seguente diagramma si può ricavare il termine β.
Quindi possiamo riscrivere il tempo di salita come:
A questo punto imponendo i valori del tempo di salita e di assestamento secondo le specifiche di progetto, utilizzando le relazioni del tr e ts si ricavano i valori di ξ e ωn da cui i parametri Kp, Kd e Ki del PID.
Metodo alternativo
Il secondo metodo di taratura del PID è stato quello empirico, queste tipologie di tecniche si utilizzano in tutti quei casi in cui le informazioni sul modello non sono sufficienti per applicare le altre tecniche di sintesi. Regole di base da seguire per una taratura empirica
Partendo dal presupposto che la struttura del PID è piuttosto definita. Le prestazioni del controllo sono quindi determinate essenzialmente dal valore dei parametri del regolatore. È possibile fornire dei criteri generali per comprendere il significato dei singoli parametri nel determinare un certo tipo di prestazione. Nel caso di algoritmo PI, ad esempio, alti valori della costante proporzionale Kp aumentano la rapidità della risposta, ma allo stesso tempo ne diminuiscono la stabilità, con il rischio di innescare pericolose oscillazioni. Valori elevati della costante di tempo dell’azione integrale Ti aiutano a stabilizzare la risposta, peggiorandone però la rapidità. In generale è possibile affermare che: • bassi valori di Kp e di Ti forniscono una risposta lenta con oscillazioni smorzate e lunghe; • un valore basso di Kp accoppiato ad un valore elevato di Ti fornisce una risposta lenta e senza sovraelongazioni; • un valore elevato di Kp accoppiato ad un valore basso di Ti fornisce una risposta rapida ma con elevate sovraelongazioni; • valori elevati sia per Kp che per Ti forniscono una risposta rapida ma con numerose oscillazioni smorzate.
Per quanto riguarda l’azione derivativa, spesso essa non viene utilizzata in quanto la sua taratura è piuttosto ostica. È possibile affermare, però, che la sua presenza permette di migliorare le prestazioni dinamiche pur mantenendo energiche le azioni proporzionale ed integrale, ossia consente di conservare alti valori di Kp e bassi valori di Ti senza correre il rischio di instabilità della risposta. Ciò richiede però cautela ed esperienza nel dimensionare correttamente la costante di tempo Td. Bisogna sottolineare che, tali considerazioni permettono di trovare manualmente i valori dei parametri che più si adattano ad un dato sistema di controllo; ma una taratura sperimentale risulta però parecchio onerosa: ogni tentativo richiede una prova e, se le costanti di tempo in gioco sono elevate, il tutto può richiedere molto tempo, senza contare il fatto che è troppo legata all’abilità e all’esperienza dell’operatore.
Il Simulatore
Come già accennato, lo scopo del nostro lavoro è stato quello di implementare un motore controllato in coppia da inserire fra i modelli di motore forniti dal Dynamic Robotic Simulator. A tal fine, abbiamo inizialmente sviluppato in ODE, servendoci dell’interfaccia grafica integrata DrawStuff, un modello semplificato di un motore angolare che, successivamente, abbiamo integrato nel simulatore vero e proprio.
Dynamic Robotic Simulator Il simulatore in questione, scritto in C++, sfrutta per il suo funzionamento componenti software quali ODE, OSG,COLLADA-DOM. La principale caratteristica del C++, com’è noto, è la possibilità di avere un approccio object-oriented. Questo ha portato alla definizione in termini di classi di tutti i componenti software che costituiscono l'architettura del simulatore. Alla luce di ciò, per ottenere il motore controllato in coppia, è stato necessario modificare le classi Simulator, Motor e Tribot, etc.. Prima di analizzare le modifiche apportate è opportuno dare una breve descrizione delle classi principali. Classe SIMULATOR Il componente principale, che racchiude tutti gli altri, è la classe Simulator. Questa classe mette a disposizione operazioni quali: caricamento di un file di configurazione, inizio e fine della simulazione, avanzamento step by step.
Classe SIMULABLE Questa classe rappresenta la base di ogni elemento che costituisce la simulazione. Essa conterrà al suo interno il modello fisico, quello visuale ed una serie di funzioni che permettono di interagire a livello fisico e visivo. Questa classe raggruppa, inoltre, la gran parte delle proprietà degli oggetti della simulazione poiché, data la natura del motore dinamico ODE, le caratteristiche che riguardano ogni tipo di modello utilizzato nella simulazione hanno molte cose in comune.
Classe PhysicalModel e VisualModel Queste classi contengono rispettivamente il modello fisico ed il modello visuale dell'oggetto simulato. Entrambe sono poco più che un contenitore per il modello visuale (un grafo OSG) e per il modello fisico (una serie di oggetti RigidBody e RigidConstraint), ma la classe PhysicalModel e più “nutrita” poiché contiene il codice necessario a passare dalla rappresentazione COLLADA alla rappresentazione interna del modello fisico. Classe Motor Questa classe si occupa della creazione dei motori. I tipi di motore utilizzabili all'interno del simulatore sono: motore controllato in coppia, motore controllato in velocità e motore controllato in posizione.
Modifiche apportate
Come accennato pocanzi, le modifiche da noi apportate si sono limitate alle classi simulator, motor e tribot. Partendo dalle classi che hanno subito più modifiche si ha: Classe Motor
E’ la classe che ha subito più modifiche. Infatti si è cominciato con l’ereditare la classe TorqueMotor, creando così la classe PosTorqueMotor PosTorqueMotor::PosTorqueMotor(RigidConstraint * joint, double init) : TorqueMotor(joint,init)
Il cui scopo è quello di simulare tramite l’implementazione di un PID all’interno del metodo void PosTorqueMotor::TakeSimulationStep(double dt)
un motore controllato in coppia. Per l’implementazione vedere l’appendice
Tribot
Le modifiche della classe Tribot riguardano essenzialmente la sostituzione del nuovo costruttore di tutti i giunti del manipolatore. In particolare, al costruttore del vecchio motore (PositionMotor) si è sostituito il nuovo (PosTorqueMotor), in modo da assegnare il suddetto motore ai giunti desiderati. RigidConstraint * joint; joint = FindJoint("Joint0-RigidConstraint"); if(joint) { //righthandmotor = new PositionMotor(joint, 0.0, _worldid); righthandmotor = new PosTorqueMotor(joint, 0.0); AddMotor( righthandmotor ); }
Simulator
Per quanto riguarda questa classe, l’unica modifica è stata quella di cambiare il riferimento, per far si che ogni qualvolta venga richiamato un position motor si ottenga un torque motor.
Problemi riscontrati Prima di procedere all’esposizione dei risultati ottenuti, è doveroso da parte nostra illustrare i problemi riscontrati e che ci hanno fatto scegliere dei modi operandi piuttosto che altri. Infatti come si potrà evincere dalle seguenti figure, a causa dei valori di CFM e ERP, gli end effector del tribot penetrano nel terreno, innescando delle “forze di reazione” tali da compromettere l’intera simulazione.
Osservando le figure si nota che, subito dopo lâ&#x20AC;&#x2122;attraversamento del terreno da parte dellâ&#x20AC;&#x2122;end effector, si ha una reazione del terreno tale da impedire il corretto inseguimento del riferimento. Nella fattispecie, grazie ad una opportuna taratura dei PID (empirica) accoppiata ad una serie di accorgimenti (sollevamento del manipolatore in fase di climbing di 7 gradi rispetto al terreno e modifica dei parametri erp e cfm) si ottiene nella stragrande maggioranza dei casi solo un non perfetto inseguimento delle traiettorie.
Analisi risultati I risultati, che verranno presentati di seguito, sono stati ottenuti con PID tarati empiricamente, infatti diversamente dal motore angolare, dove il PID segue abbastanza bene i riferimenti, nel tribot simulator i risultati ottenuti con la taratura analitica non sono stati altrettanto soddisfacenti . Motore angolare
Kd
Kp
Ki
-0.1453
0.057
0.41
Taratura empirica
Al fine di ottenere un controllo ottimale del tribot, si è scelto di implementare un PID per ogni giunto costituente il robot.
Valori dei guadagni del PID per ogni giunto
Giunti 0-1 2-3 4-5 6
Nome giunto Left/right handmotor Left/right armmotor Left/right shouldermotor Manipmotor
Kd 0.02
Kp 0.99
Ki 0.06
0.022
0.599
0.035
0.022
0.599
0.045
0.025
0.99
0.016
Home position + Sensor position
Zoom dello Spike di coppia
Successivamente è riproposta la fase di Home + Sensor position, aumentando però il numero di osservazioni del passaggio da home a sensor position e viceversa.
Climb
Come si evince ora il tribot segue abbastanza fedelmente i riferimenti dati nonostante i compromessi a cui si è giunti per evitare le cosiddette “ esplosioni “, resta ancora però da correggere, come si può notare dal grafico raffigurante la coppia in “home position + sensor”, uno spike di coppia. Tale spike può essere ridotto tramite un’opportuna taratura dei PID dei giunti 2-3 (Left/rightarmmotor) , a scapito però della robustezza.
Appendice A
In questa sezione sono presentati i listati delle modifiche da noi apportate.
MOTORE ANGOLARE #include <ode/ode.h> #include <drawstuff/drawstuff.h> #include<stdio.h> #include<math.h> #define Pi 3.14159 #define TORQUEMAX 1.62 #ifdef MSVC #pragma warning(disable:4244 4305) complaints #endif
// for VC++, no precision loss
// select correct drawing functions #ifdef dDOUBLE #define dsDrawBox dsDrawBoxD #define dsDrawSphere dsDrawSphereD #define dsDrawCylinder dsDrawCylinderD #define dsDrawCappedCylinder dsDrawCappedCylinderD #endif FILE *f=fopen("coppia.txt","w+"); FILE *f2=fopen("time.txt","w+"); FILE *f3=fopen("errore.txt","w+"); FILE *f4=fopen("angolo.txt","w+"); FILE *f5=fopen("angolorate.txt","w+"); FILE *f6=fopen("angodes.txt","w+"); FILE *f7=fopen("e_int.txt","w+"); // global constants const dReal ERP = 0.2; const dReal CFM = 0.05; const dReal HEAD = 0.12; const dReal TORSO[3] = {0.1,0.1,.40}; //0.7 const dReal STARTZ = 5.0;
// length, width height
// environment setup static dWorldID world; static dGeomID ground, geom_group; static dJointGroupID contactgroup; static dSpaceID space; static dBodyID body[11],head; // 11 body parts + head static dGeomID box[11]; // 11 geometries for body parts
static static static static static
dJointID dJointID dJointID dJointID dJointID
joint[10],headj; lmotor[2]; amotor[2]; head_ball; head_motor;
static dJointID hook; static dReal target[5]={0,90,180,-90,0}; static dReal target1[5]={0,90,180,270,360}; static dReal tar; static dReal tar1; int i=0; static dReal Ikmax=0.2; dReal jIntErr=0; static dReal Dk=-0.1453;//0.0238;//0.1522;//0.5300;//0.2293;// 0.1522;//0.1472;//-0.14412; static dReal Pk=0.057;//21.4222;//63.5785;//15.5379;//37.3075;//63.5785;//0.02719;//0.09016; static dReal Ik=0.41;//10.5260;//0.1050;//3.7011;//7.8539;//0.1050;//0.2238;//0.8232; static dReal prev_dAngle = 0; // Collision call back function here static void nearCallback (void *data, dGeomID o1, dGeomID o2) { } // start simulation - set viewpoint static void start() { static float xyz[3] = {0.8317f,-0.9817f,5.8000f}; static float hpr[3] = {121.0000f,-27.5000f,0.0000f}; dsSetViewpoint (xyz,hpr); } dReal getAMotorAngleRate (dJointID joint, int anum) { dReal rate = 0; dBodyID b0 = dJointGetBody(joint, 0); dBodyID b1 = dJointGetBody(joint, 1); dVector3 axis; dJointGetAMotorAxis (joint, anum, axis); if (b0 != 0) { const dReal *avel0 = dBodyGetAngularVel(b0); rate += axis[0]*avel0[0] + axis[1]*avel0[1] + axis[2]*avel0[2]; } if (b1 != 0) { const dReal *avel1 = dBodyGetAngularVel(b1); rate -= axis[0]*avel1[0] + axis[1]*avel1[1] + axis[2]*avel1[2]; } if (b0 == 0 && b1 != 0) rate = -rate; return rate; }
// called when a key pressed static void command (int cmd)
{ switch(cmd) { case 'o':i=i+1; tar1=target1[i]; if(i==4) i=0; break; } }
//implementazione del PID double Control(static int steps){ dReal dAngle=tar1; dReal torque,prop; dReal jAngle = dJointGetAMotorAngle(head_motor,2); dReal jAngRate = getAMotorAngleRate(head_motor,2); if( prev_dAngle != dAngle) { jIntErr = 0; } prev_dAngle= dAngle;//update latest desired angle for the joint
if(jAngle<0){// aggiusto la seconda metĂ ma resta il problema del ritorno a 0 jAngle=jAngle+2*M_PI;}
if((dAngle*M_PI/180-jAngle)>5)//corregge l'errore che si ha in zero prop = -1*jAngle; else //proportional difference. prop = dAngle*M_PI/180 - jAngle;
//integrate error term and assign torque jIntErr = jIntErr + prop; if( jIntErr > Ikmax)//integral term saturates jIntErr = Ikmax; if(jIntErr<=0) jIntErr=0; torque = Pk*(prop)+Dk*jAngRate+Ik*jIntErr; //saturazione coppia if(torque>TORQUEMAX) torque=TORQUEMAX; if(torque<-TORQUEMAX) torque=-TORQUEMAX;
dJointAddAMotorTorques(head_motor, 0, 0,torque);
//salvataggio dati fprintf(f2,"%d\n ",steps); fprintf( f,"%6.2f\n", torque); fprintf( f3,"%6.2f\n", prop); fprintf( f4,"%6.2f\n", jAngle); fprintf( f5,"%6.2f\n", jAngRate); fprintf( f6,"%6.2f\n", dAngle); fprintf( f7,"%6.2f\n", jIntErr); return torque; }
// run Simulation static void simLoop(int pause) { static int steps=0; static int itime = 0; static int vsign = 1; dSpaceCollide (space,0,&nearCallback); dWorldStep (world,0.005); // remove all contact joints dJointGroupEmpty (contactgroup); dsSetColor (0,1,1); dsSetTexture (DS_WOOD); // DS_WOOD // draw components dsDrawSphere(dBodyGetPosition(head),dBodyGetRotation(head),HEAD); dsSetColor (1,0,1); dsDrawBox(dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),TORSO);
double tor=Control(steps);
steps++; } static void init(void) { dMass m; for (int i=0;i<11;i++) body[i] = dBodyCreate(world); head = dBodyCreate(world); dBodySetPosition (head,0,0,STARTZ+TORSO[2]/2+HEAD); dBodySetPosition (body[0],0,0,STARTZ); // torso // head dMassSetSphere(&m,1,HEAD); dBodySetMass(head, &m); // torso dMassSetBox (&m,1,TORSO[0],TORSO[1],TORSO[2]); // torso dMassAdjust (&m,1); // torso weight dBodySetMass (body[0],&m); box[0] = dCreateBox (space,TORSO[0],TORSO[1],TORSO[2]); dGeomSetBody (box[0],body[0]);
// Create Joints // hook hook = dJointCreateFixed (world, 0); dJointAttach (hook, 0, head); dJointSetFixed (hook); // head joint head_ball = dJointCreateBall(world,0); dJointAttach(head_ball,body[0],head); dJointSetBallAnchor(head_ball,0,0,STARTZ+TORSO[2]/2); head_motor = dJointCreateAMotor(world,0); dJointAttach(head_motor,body[0],head); const float stop = Pi/2; // const float stop_cfm = 0.01; // const float stop_erp = 0.8; const float stop_cfm = 0.05; const float stop_erp = 0.2; dJointSetAMotorMode(head_motor,dAMotorEuler); dJointSetAMotorAxis(head_motor,0,1,0,0,1); dJointSetAMotorAxis(head_motor,2,2,0,1,0); dJointSetAMotorParam(head_motor,dParamLoStop,-stop); // twist dJointSetAMotorParam(head_motor,dParamHiStop,stop); dJointSetAMotorParam(head_motor,dParamVel,0); dJointSetAMotorParam(head_motor,dParamFMax,0); dJointSetAMotorParam(head_motor,dParamLoStop2,-stop); // back dJointSetAMotorParam(head_motor,dParamHiStop2,stop); // front dJointSetAMotorParam(head_motor,dParamVel2,0); dJointSetAMotorParam(head_motor,dParamFMax2,0);
} // main program int main(int argc, char **argv) { // setup pointers to drawstuff callback functions dsFunctions fn; fn.version = DS_VERSION; fn.start = &start; fn.step = &simLoop; fn.command = &command; fn.stop = 0; fn.path_to_textures = "../../drawstuff/textures"; // create world (setup environment) world = dWorldCreate(); // create ODE world space = dHashSpaceCreate(space); contactgroup = dJointGroupCreate (0); //dWorldSetGravity (world,0,0,-9.80); // set gravity (x,y,z) dWorldSetERP (world,ERP); // global ERP dWorldSetCFM (world,CFM); // global CFM ground = dCreatePlane(space,0,0,1,0); init(); // Start Simulation dsSimulationLoop (argc,argv,640,480,&fn); // 640 x 480 OpenGL //display // Destroy World (end program)
dJointGroupDestroy (contactgroup); dSpaceDestroy (space); dWorldDestroy(world); return 0; }
APPENDICE B
Di seguito verranno elencate, in dettaglio, le modifiche da noi apportate File Motor.cc Osservazione
La funzione setParam(dParamFMax, 1.1) introduce una sorta di coppia resistente, in quanto abbiamo riscontrato sperimentalmente nessun giunto riusciva a muoversi se non prima la coppia erogata dal PID superava quella impostata dal suddetto parametro /*Oggetto della tesina è stato lo sviluppo e l'implementazione della classe PosTorqueMotor ottenuta ereditando metodi e oggetti dalla classe TorqueMotor.*/
//Costruttore della classe PosTorqueMotor PosTorqueMotor::PosTorqueMotor(RigidConstraint * joint, double init) : TorqueMotor(joint,init){ angle=0.0; torque =0.0; jIntErr=0.0; dHingeJoint * hingejoint = static_cast<dHingeJoint *>( rc->djoint );
} //Metodo che ritorna il tipo di Motore MotorType PosTorqueMotor::GetType() { if(!rc) return RobotSimulator::INERROR; return RobotSimulator::TORQUE; } /*Metodo ResetVar() consente di resettare le variabili: jIntErr-> errore integrale tor->coppia jAngRate->velocitĂ angolare dAngle->riferimento dell'angolo prop->errore sull'angolo */
void PosTorqueMotor::ResetVar(){ jIntErr=0; tor=0; jAngRate=0; dAngle=0; prop=0; } //Consente di settare l'angolo di riferimento void PosTorqueMotor::SetAngle(double r) { angle = r; if( angle > M_PI ) angle -= 2.0 * M_PI; }
// Ritorna l'angolo di riferimento double PosTorqueMotor::GetAngle() { return angle; } /*All'interno di TakeSimulationStep sono implementati i PID utili al controllo di coppia*/ void PosTorqueMotor::TakeSimulationStep(double dt) { /*Vengono definite con i valori ottenuti mediante taratura empirica dei PID le costanti Dk (derivativa) Pk (proporzionale) e Ik(integrativa)*/ static dReal Dk; static dReal Pk; static dReal Ik; // dHingeJoint * hingejoint = static_cast<dHingeJoint *>( rc->djoint ); if(rc->sid[5]=='6'){ // manipmotor /*Per ottenere un controllo più preciso si è ritenuto opportuno distinguere i valori dei coefficienti Kp Kd e Ki in funzione dell'Id del giunto e cioè dell'ubicazione del giunto stesso.*/ Dk=0.025; Pk=0.99; Ik=0.016; } else if(((rc->sid[5]=='0')||(rc->sid[5]=='1'))){// left/right hand motor
Dk=0.02; Pk=0.99; Ik=0.06; }
else if(((rc->sid[5]=='2')||(rc->sid[5]=='3'))){//right/left arm motor Dk=0.022; Pk=0.599; Ik=0.035; } else if(((rc->sid[5]=='4')||(rc->sid[5]=='5'))){//right/left shoulder motor
Dk=0.022; Pk=0.599; Ik=0.045; } //In questa fase vengono prima valutati la posizione attuale e la velocità angolare del giunto. dAngle=angle; dReal jAngle = GetActualAngle(); jAngRate = hingejoint->getAngleRate(); //viene poi calcolato l'errore tra l'angolo attuale e il riferimento prop = dAngle - jAngle; //Che viene sommato all'errore jIntErr jIntErr = jIntErr + prop; //E infine è calcolato il valore della coppia: tor = Pk*(prop)-Dk*jAngRate+Ik*jIntErr;
if((rc->sid[5]=='2')||(rc->sid[5]=='3')||(rc->sid[5]=='6')) printf("id %c\t perr: %6.2f\t Ierr=%6.2f \t derr:%6.2f\t tor: %6.2f\n",rc->sid[5],prop,jIntErr,jAngRate,tor);
//Che viene assegnata mediante il metodo SetTorque(tor) TorqueMotor::SetTorque(tor); TorqueMotor::TakeSimulationStep(dt); }
File tribot.cc void Tribot::SetupRobot() { RigidConstraint * joint; /*Ai giunti 0-1-2-3-4-5-6 del Tribot sono stati assegnati motori della classe PosTorqueMotor*/ joint = FindJoint("Joint0-RigidConstraint"); if(joint) { //righthandmotor = new PositionMotor(joint, 0.0, _worldid); righthandmotor = new PosTorqueMotor(joint, 0.0); AddMotor( righthandmotor ); } joint = FindJoint("Joint1-RigidConstraint"); if(joint) { //lefthandmotor = new PositionMotor(joint, 0.0, _worldid); lefthandmotor = new PosTorqueMotor(joint, 0.0); AddMotor( lefthandmotor ); } joint = FindJoint("Joint2-RigidConstraint"); if(joint) {
//rightarmmotor = new PositionMotor(joint, 0.0, _worldid); rightarmmotor = new PosTorqueMotor(joint, 0.0); AddMotor( rightarmmotor ); } joint = FindJoint("Joint3-RigidConstraint"); if(joint) { //leftarmmotor = new PositionMotor(joint, 0.0, _worldid); leftarmmotor = new PosTorqueMotor(joint, 0.0); AddMotor( leftarmmotor ); } joint = FindJoint("Joint4-RigidConstraint"); if(joint) { //rightshouldermotor = new PositionMotor(joint, 0.0, _worldid); rightshouldermotor = new PosTorqueMotor(joint, 0.0); AddMotor( rightshouldermotor ); } joint = FindJoint("Joint5-RigidConstraint"); if(joint) { //leftshouldermotor = new PositionMotor(joint, 0.0, _worldid); leftshouldermotor = new PosTorqueMotor(joint, 0.0); AddMotor( leftshouldermotor ); } joint = FindJoint("Joint6-RigidConstraint"); if(joint) { //manipmotor = new PositionMotor(joint, 0.0, _worldid); manipmotor = new PosTorqueMotor(joint, 0.0); AddMotor( manipmotor ); }
void Tribot::MoveDownManipulator() { if(antmodule && executing == NONE) {
manipexecuting = MOVE; manipmotor->SetAngle(TORADIANS(7.0));//-20 per evitare di sovraccaricare il manipolatore lo si alza di 7 gradi
APPENDICE C
Per ovviare ai problemi precedentemente esposti, si è pensato di proporre la tecnica di controllo di cedevolezza spiegata successivamente.
Interazione del manipolatore con l’ambiente 4 Nella fase di interazione, l’ambiente impone dei vincoli stringenti ai percorsi geometrici che devono essere seguiti dall’organo terminale del manipolatore, è usuale riferirsi a questa situazione con la dizione di moto vincolato. La corretta esecuzione del compito di interazione con l’ambiente impiegando algoritmi di controllo del moto richiederebbe una perfetta pianificazione del compito, sia dal punto di vista del manipolatore ( in termini di modello cinematico e dinamico ) che dell’ambiente in cui si trova ad operare. Un errore di pianificazione, infatti dà luogo a forze di contatto che provocano uno scostamento dell’organo terminale dalla traiettoria desiderata. D’altro canto il sistema reagisce allo scopo di ridurre l’entità dello scostamento. Questa situazione conduce ad una crescita del valore della forza di contatto che trova il limite solo per l’intervento di una saturazione negli attuatori o per subentrata crisi meccanica di uno degli elementi. Tale inconveniente può essere superato assicurando per il manipolatore un comportamento cedevole durante l’interazione.
4
Sciavicco Siciliano-Robotica Industriale Cap.7
Controllo di cedevolezza 4
A partire dal modello dinamico del manipolatore nello spazio operativo, dove h è il vettore delle forze di contatto esercitate dall’organo terminale del manipolatore sull’ambiente:
Impiegando una legge di controllo PD con compensazione di gravità nello spazio operativo:
Si ottiene all’equilibrio:
Il manipolatore a contatto, sotto l’azione del controllo di postura si comporta nello spazio operativo come una molla generalizzata di nei riguardi di . cedevolezza
Inoltre è possibile semplificare l’analisi considerando un modello di contatto in un ambiente elasticamente cedevole, descritto dal modello seguente: 4
Sciavicco Siciliano - Robotica Industriale Capitolo 7
Con
spostamento generalizzato espresso in coordinate di spazio
operativo,rispetto alla posizione di equilibrio dell’ambiente non deformato
Dalle relazioni successive è possibile legare le forze equivalenti impresse al manipolatore con le deformazioni subite dall’ambiente tramite la ovvero la matrice di rigidezza del’ambiente. matrice La matrice
rappresenta la cedevolezza attiva poiché è realizzata sul
manipolatore tramite l’azione di una opportuna legge di controllo di posizione. Con il modello dell’ambiente diventa:
All’equilibrio la posizione dell’organo terminale sarà:
E la forza di contatto è:
L’interazione dei due sistemi è influenzata dal peso associato alle rispettive caratteristiche di cedevolezza. E’ infatti possibile agire sulla cedevolezza in maniera tale da far dominare il manipolatore sull’ambiente o viceversa. La dominanza può essere specificata con riferimento alle singole direzioni dello spazio operativo per una data rigidezza dell’ambiente in dipendenza del compito di interazione corrispondenti specificato si scelgono valori elevati per gli elementi di alle direzioni in cui si desidera che l’ambiente ceda e valori bassi per gli corrispondenti alle direzioni in cui è il manipolatore a elementi di dover cedere. Dall’espressione della forza di contatto è possibile accordare le caratteristiche di cedevolezza del manipolatore a quelle
dell’ambiente,che può presentare caratteristiche differenti lungo direzioni diverse dello spazio operativo.
Appendice D Dynamixel AX12+
Il Dynamixel AX12+ è un potente servo motore che grazie ai numerosi accessori in dotazione può essere agevolmente utilizzato nella costruzione di robot.
Le caratteristiche di base possono essere riassunte nella seguante tabella AX12+ Input voltage
7V
10v
Max Holding Torque(Nm)
1.18Nm
1.62Nm
60degree time (s)
0.269s
0.196s
References
[1] Marco Cosentino, Un ambiente dinamico per la simulazione comportamentale di bio-robot [2] Documentazione ODE http://www.ode.org/ode-latest-userguide.html [3] An ODE and Robotics Related Site by Kosei Demura http://demura.net/english [4] Sciavicco Lorenzo, Siciliano Bruno - Robotica Industriale,02/2003 The McGraw-Hill Companies