1. Real Time Control of an Anthropomorphic Robotic Arm using FPGA
Advisor:
Prof. Ciro Natale
Students:
Francesco Castaldo
Andrea Cirillo
Pasquale Cirillo
Umberto Ferrara
Luigi Palmieri
Altera InnovateItaly Design Contest 2011
01/12/2011
Real Time Control of an Anthropomorphic Robotic Arm using FPGA

2. Objective Introduction Some applications
The project consists to make an anthropomorphic robotic arm controlled in real-time by user with a wireless controller.
The whole system is FPGA-based and it doesn’t use a personal computer.
Some applications
The idea is to realize a low cost control system that can be used in some critical applications:
Rescue missions;
Remote manipulation.
Il progetto che andremo a presentare consiste nella realizzazione di un braccio antropomorfo a sei gradi di libertà con polso sferico e la realizzazione del relativo sistema di controllo basato, appunto sull’FPGA ALTERA. L’idea è stata, principalmente, quella di realizzare un sistema a basso costo utilizzabile in applicazioni critiche quali possono essere la manipolazione di materiale pericoloso o missioni di soccorso dove l’intervento dell’uomo è alquanto complicato. Per questo si è cercato di rendere completamente indipendente l’intero sistema: l’FPGA si occupa sia della parte computazionale sia di controllo e di gestione dell’hardware senza, quindi, che fosse stato necessario l’utilizzo di un personal computer.
Altera InnovateItaly Design Contest 2011
01/12/2011
Real Time Control of an Anthropomorphic Robotic Arm using FPGA

3. Architecture Two Altera DE1 Boards; One PS/2 Keyboard;
Two Xbee Module;
An anthropomorphic robotic arm with spherical wrist (6 DOF);
An home-made optoelectronic force-sensor.
Di seguito è presente uno schema a blocchi del sistema: sono state utilizzate due DE1 Board: una per l’interfacciamento con l’utente che gestisce l’invio dei comandi al robot e una che ospita il sistema di controllo vero e proprio; una tastiera per il controllo del robot; due moduli xbee per la trasmissione wireless dei dati; un braccio antropomorfo; un sensore di forza optoelettronico utilizzato per avere un feedback sull’avvenuta presa del target desiderato. Abbiamo utilizzato due moduli wireless proprio per permettere l’utilizzo del braccio a distanza in modo che potesse essere immerso in ambienti come quelli descritti precedentemente.
Altera InnovateItaly Design Contest 2011
01/12/2011
Real Time Control of an Anthropomorphic Robotic Arm using FPGA

4. Functioning (1/2)
User sends a remote command pressing a button of the keyboard.
FPGA captures the scancode from PS/2 interface and it sends the command byte to the Xbee module.
Vediamo in dettaglio il funzionamento dell’intero sistema:
L’utente che pilota il braccio invia i comandi attraverso la keyboard;
L’FPGA utilizzato per l’interfacciamento con l’utente cattura lo scancode proveniente dall’interfaccia PS/2 ed invia un comando, ossia un singolo byte per volta, al secondo FPGA tramite il modulo XBEE;
I moduli XBEE, trasmettitore e ricevitore, sono stati adeguatamente programmati in modo da realizzare una rete peer-to-peer costituita appunto dai soli due dispositivi. I due moduli forniscono semplicemente un ponte wireless: i due FPGA è come se comunicassero tramite lo standard RS232 su cavo.
The transmitter Xbee module sends the information to the receiver module.
Altera InnovateItaly Design Contest 2011
01/12/2011
Real Time Control of an Anthropomorphic Robotic Arm using FPGA

5. Functioning (2/2)
The Xbee Module sends the received command to FPGA control unit.
FPGA elaborates information, changes the wirst position, resolves the inverse kinematic algorithm.
FPGA updates the PWM signals for the seven servo motors.
Il carattere inviato dal modulo trasmettitore viene ricevuto dal modulo ricevitore che passa l’informazione all’FPGA che implementa il controllo;
L’FPGA elabora l’informazione e modifica il punto di polso del manipolatore risolvendo l’algoritmo di cinematica inversa implementato;
Per permettere il movimento di ogni singolo giunto viene generato un segnale PWM per ogni servomotore in base alla posizione che esso deve raggiungere;
Se è stato inviato un comando di presa il gripper viene chiuso gradualmente fin quando non viene restituito il feedback dal sensore posto in punta che informa, appunto, circa l’avvenuta operazione di presa.
The arm moves in real-time and can receive a feedback from the sensor mounted on the end-effector.
Altera InnovateItaly Design Contest 2011
01/12/2011
Real Time Control of an Anthropomorphic Robotic Arm using FPGA

6. Soft-Core NIOS II (1/2) FPGA for User Interface
SRAM controller
NIOS II/S Processor
PS/2 Controller
UART (RS232 Serial Port)
Analizziamo ora i due Soft-Core utilizzati per i due FPGA. Partendo da quello utilizzato per l’interfacciamento con l’utente vediamo che oltre al processore sono stati utilizzati un modulo di SRAM, una porta UART per la comunicazione con il modulo XBEE, il controller PS/2 per l’interfacciamento con la tastiera, e la JTAG utilizzata nella fase di debugging dell’applicazione in quanto su di essa è stato reindirizzato lo stream di I/O e per la programmazione. Data la semplicità dell’architettura si può notare come sia stato occupato solo il 20% della memoria disponibile.
JTAG UART
FPGA for User Interface
Altera InnovateItaly Design Contest 2011
01/12/2011
Real Time Control of an Anthropomorphic Robotic Arm using FPGA

7. Soft-Core NIOS II (2/2) FPGA for Control Unit
NIOS II/F Processor
Interval Timer
Parallel I/O Interface for Sensor signal
SRAM controller
UART (RS232 Serial Port)
Parallel I/O Interface for PWM signals
Per quanto riguarda l’FPGA adibito al controllo del manipolatore sono stati implementati un modulo timer che scandisce i tempi di aggiornamento dei segnali PWM, la JTAG utilizzata come nel caso precedente, un modulo UART per la comunicazione con il modulo XBEE e l’interfaccia parallela sui cui pin sono stati inviati i segnali PWM ed è acquisito il segnale proveniente dal sensore. Rispetto al caso precedente in cui è stato utilizzato un processore NIOS II/S data la complessità dei calcoli da svolgere per la risoluzione dell’algoritmo di cinematica inversa è stato utilizzato un processore NIOS II/F, più performante ma che certamente richiede più spazio in memoria. E come si può vedere dall’immagine l’occupazione di memoria è del 27%, maggiore che nel caso precedente.
JTAG UART
FPGA for Control Unit
Altera InnovateItaly Design Contest 2011
01/12/2011
Real Time Control of an Anthropomorphic Robotic Arm using FPGA

8. Inverse Kinematic (1/2)
The inverse kinematic problem is difficult to solve:
Non-linear equations (sine, cosine in rotation matrices);
The existence of multiple solutions;
The possible non-existence of a solution;
Singularities.
IK Simplifications:
Decouple the problem into independent subproblems:
determining the inverse solution to the problem of positioning;
determining the inverse solution to the problem of orientation.
Prima di illustrare com’è stato implementato l’intero sistema, facciamo un piccolo accenno al problema della cinematica inversa. Essa viene utilizzata per ricavare gli angoli ai giunti del manipolatore partendo dal punto dello spazio di lavoro che si vuole raggiungere espresso in coordinate cartesiane. L’algoritmo utilizzato si rifà alla risoluzione analitica in forma chiusa del problema e richiede la risoluzione di equazioni non lineari in cui compaiono seni e coseni. In più c’è da gestire il caso in cui possano esserci soluzioni multiple al problema o singolarità.
Il problema, però, può essere semplificato dividendolo in due sottoproblemi indipendenti:
Uno consiste nel risolvere il problema di posizionamento della struttura portante;
Il secondo nel determinare l’orientamento del polso.
Altera InnovateItaly Design Contest 2011
01/12/2011
Real Time Control of an Anthropomorphic Robotic Arm using FPGA

9. Inverse Kinematic (2/2) Altera InnovateItaly Design Contest 2011
L’algoritmo da noi utilizzato, a partire dal punto di polso calcola gli angoli ai giunti costituenti i primi tre gradi di libertà del manipolatore. L’orientamento dell’organo terminale è lasciato all’utente che tramite dei comandi da tastiera può ruotare i tre giunti del polso sferico. In questo modo è stato fornito all’utilizzatore un ulteriore grado di libertà per permettere un’operazione di presa del tutto generale dell’oggetto. C’è da notare che nel caso in cui gli angoli teta4,5,6 fossero stati calcolati tramite l’algoritmo di cinematica il manipolatore avrebbe fornito all’utente sempre gli stessi versori di approccio e scivolamento, in modo da non permettergli di raggiungere oggetti posizionati nello spazio nel modo più casuale possibile.
Altera InnovateItaly Design Contest 2011
01/12/2011
Real Time Control of an Anthropomorphic Robotic Arm using FPGA

10. The Arm Servos: Mechanical structure:
One HS-485 (Torque 6 Vdc) for the base: it gives to structure the possibility to rotate around the vertical axis;
Two HS-755HB (Torque 13.2 Vdc) for the shoulder;
One HS-755HB (Torque 13.2 Vdc) for the elbow;
One HS-485 (Torque 6 for the wrist;
Two HS-422 (Torque 4.1 Vdc) for two additional DOF for the spherical wrist;
One HS-485 (Torque 6 6Vdc) to control the gripper.
Mechanical structure:
A glass cylindrical base with ball bearings;
Two multi-purpose aluminum bracket for Maxi Servos used in shoulder and elbow joints;
A multi-purpose aluminum bracket for Standard Servos (HS-485) for the wrist joint;
Two C-clamps for Maxi Servos with cylindrical bearings and two aluminum joints to connect the shoulder and elbow joints each other, through a tube 6 cm;
Two L-brackets, two aluminum joints, a tube of 6 cm for the realization of the forearm, so the connection between the wrist joint and elbow joint;
Two low-profile axes for the implementation of two of the three degrees of freedom of the spherical wrist. They were connected to two ball bearings to reduce friction of rotation;
As end effector, a simple plastic caliper (Little Grip) is used;
Extensions of various lengths for the servo motors cables.
Il braccio è stato realizzato tramite componenti utilizzati nell’ambito della modellistica. Data l’elasticità di tali oggetti quali servomotori, squadre in plastica, componenti meccanici in alluminio leggero e cuscinetti a sfera economici la struttura portante risultante non riesce a smorzare le vibrazioni generate dal movimento dei motori né riesce ad opporsi alla forza di gravità soprattutto quando il braccio è in piena estensione. Per questo è stata utilizzata una molla di compensazione per ridurre gli effetti citati. Per ogni giunto è stato utilizzato, come detto, un servomotore la cui coppia è stata dimensionata in base al carico complessivo che esso deve sopportare. Solo alla base anziché montare un solo servomotore, ne sono stati usati due montati in modo da raddoppiare la coppia a disposizione su quel giunto. Per alimentare tutti i 7 motori che in picco e in stallo assorbono circa 5 A, è stato utilizzato un alimentatore per pc che sulla linea da 5V riesce ad erogare fino a 30 A nominali.
Altera InnovateItaly Design Contest 2011
01/12/2011
Real Time Control of an Anthropomorphic Robotic Arm using FPGA

11. Interfacing FPGAs
We use the expansion headers of the DE1 Board development kit (GPIO_0 and GPIO_1) to interface the FPGAs with the XBEE modules and with the arm.
Two boards have been made:
One for the manipulator and the FPGA that handles the control signals for servo motors;
Another one for the FPGA on which the controller is implemented.
Il collegamento dei due FPGA ai moduli XBEE e al manipolatore è stato realizzato mediante delle schede sviluppate su piastra millefori di cui riportiamo gli schematici, il PCB e una foto che ne ritrae, appunto, la versione realizzata. Nel caso dell’FPGA per l’interfacciamento con l’utente la scheda consiste di un connettore a 40 poli, e l’alloggiamento per il modulo XBEE. Nel caso dell’FPGA che implementa il controllo la scheda prevede due connettori a 40 poli, un molex per prelevare l’alimentazione per i motori, i connettori per i servomotori, un regolatore lineare di tensione che fornisce l’alimentazione al modulo XBEE e un connettore per il collegamento del sensore di forza. Abbiamo pensato ai connettori a 40 poli poiché permettevano un facile collegamento dei dispositivi mediante dei cavi flat a 40 poli.
Altera InnovateItaly Design Contest 2011
01/12/2011
Real Time Control of an Anthropomorphic Robotic Arm using FPGA

12. Force Sensor
The sensor developed for the gripper provides information about the successful operation of grasping:
It estimates the contact force;
Simply, comparing the voltage value with a predetermined threshold voltage, it gives information about the contact between two bodies.
Il sensore di forza è stato sviluppato in tecnologia optoelettronica utilizzando, per ogni cella, un led ed un fototransistor e due resistori adeguatamente dimensionati. Il resistore collegato al led impone la corrente che circola in esso e quindi la quantità di fotoni emessi; il resistore collegato al fototransistor serve per stabilire il livello di tensione base del segnale acquisito. Tale sensore può essere utilizzato in due modi, sia per stimare la forza di contatto sia per fornire semplicemente un’informazione circa l’avvenuto contatto tra due oggetti. Nel primo caso c’è bisogno di una fase di calibrazione e di una rete neurale che a partire dai livelli di tensione acquisiti fornisce i livelli di forza, nel secondo basta confrontare il segnale acquisito dal fototransistor con un valore di soglia prestabilito per capire se è avvenuto un contatto o meno. Di seguito è possibile vedere lo schematico, il PCB e alcune immagini della versione del sensore realizzata su piastra presensibilizzata.
Altera InnovateItaly Design Contest 2011
01/12/2011
Real Time Control of an Anthropomorphic Robotic Arm using FPGA

13. Implementation (1/4) Principal problems: Commands acquisition;
Interfacing with Xbee Module;
Implementation of Inverse Kinematic Algorithm;
PWM Signals generation;
Management sensor feedback.
Commands acquisition:
Use PS/2 Controller;
Decode the keyboard scancode received;
alt_up_ps2_dev* alt_up_ps2_open_dev(const char *name)
void alt_up_ps2_init(alt_up_ps2_dev *ps2)
int decode_scancode(alt_up_ps2_dev *ps2, KB_CODE_TYPE *decode_mode, alt_u8 *buf, char *ascii)
Dopo aver introdotto i componenti utilizzati nel sistema, passiamo ad illustrare il codice sviluppato considerando i cinque problemi principali in cui si può dividere l’intero progetto: acquisizione dei comandi inviati dall’utente, interfacciamento con i moduli XBEE, implementazione dell’algoritmo di cinematica inversa, generazione dei segnali PWM e gestione del segnale proveniente dal sensore.
Per l’invio dei comandi da parte dell’utente, è stata utilizzata, come detto, una keyboard PS/2. Sull’FPGA impiegato per l’interfacciamento con l’utente, implementata quindi il controller PS/2, sono state utilizzate le librerie messe a disposizione dall’IDE per l’apertura di una struttura di tipo PS/2, per inizializzare il device e individuare se quel device è una tastiera o un mouse e per comunicare con la tastiera per acquisire lo scancode relativo al tasto premuto.
Altera InnovateItaly Design Contest 2011
01/12/2011
Real Time Control of an Anthropomorphic Robotic Arm using FPGA

14. Implementation (2/4) Interfacing with Xbee Module: Use UART Interface:
BaudRate: bps;
Parity: NONE;
DATA Bits: 8
Stop Bits: 1
IOWR_ALTERA_AVALON_UART_TXDATA(base, data)
Receive with UART Interrupt:
alt_irq_register(UART_IRQ, 0, uart_ISR); …
command = IORD_ALTERA_AVALON_UART_RXDATA(UART_BASE);
La comunicazione con i moduli XBEE è stata realizzata mediante la creazione di interfacce UART, i cui parametri di impostazione sono stati definiti sulla base della configurazione degli stessi moduli che, come detto, necessitano di essere configurati prima del loro utilizzo. Sull’FPGA che implementa il controllo, è stata utilizzata un’interruzione per la gestione della ricezione dei comandi nella cui ISR associata e registrata vengono modificate, sulla base del comando inviato, le coordinate del punto di polso da cui vengono calcolati gli angoli ai giunti del manipolatore tramite l’algoritmo di cinematica. Con i relativi comandi messi a disposizione dalle librerie, vengono inviati e ricevuti i caratteri associati ai comandi.
Altera InnovateItaly Design Contest 2011
01/12/2011
Real Time Control of an Anthropomorphic Robotic Arm using FPGA

15. Implementation (3/4) Implementation of Inverse Kinematic Algorithm:
Include math.h library for atan2() function, non linear sine and cosine function;
Implementation of matrix transpost function;
Implementation of matrix product function.
PWM Signals generation:
Use Timer to generate interrupt;
Use GPIO pins;
Signal frequency: 50 Hz;
Update Duty Cicle after IK algorithm execution.
alt_irq_register(TIMER_IRQ, 0, timer_ISR); …
IOWR_ALTERA_AVALON_TIMER_STATUS(TIMER_BASE, 0);
L’algoritmo di cinematica presuppone la risoluzione di operazioni matriciali quali prodotto e trasposta implementate tramite dei cicli for oltre che l’utilizzo delle funzioni seno, coseno e atan2 messe a disposizione dalla libreria matematica.
Per quanto riguarda, invece, la generazione dei segnali da inviare ai motori è stato utilizzato un timer che genera un’interruzione ogni 20 ms. All’interruzione è appunto associata una ISR in cui viene aggiornato il duty cicle di ogni segnale sulla base del nuovo punto da raggiungere. Importante è azzerare il flag associato all’interruzione in modo che alla prossima occorrenza possa essere nuovamente gestita.
Altera InnovateItaly Design Contest 2011
01/12/2011
Real Time Control of an Anthropomorphic Robotic Arm using FPGA

16. Implementation (4/4) Management sensor feedback: Use GPIO Interrupt;
Stop the motor of the grip when interrupt occurs.
alt_irq_register(SENSOR_IRQ, 0, sensor_ISR);
L’acquisizione del segnale dal sensore viene effettuata utilizzando un pin dell’expansion header. Ancora è stata utilizzata un’interruzione nella cui ISR viene fermata la corsa del motore che, quindi, si arresta mantenendo la presa una volta che l’oggetto è stato afferrato.
Altera InnovateItaly Design Contest 2011
01/12/2011
Real Time Control of an Anthropomorphic Robotic Arm using FPGA

17. The manipulator at work…
Future Developments
Add angular sensors to control arm in feedback to improve the precision of the movements.
Add camera on the grip to view the target position in the workspace.
Give to the arm the possibility to move as a mobile robot.
Replace the keyboard with a R/C controller to improve the movement flexibility.
Alcuni sviluppi futuri e modifiche che si potrebbero apportare per migliorare il sistema sono:
L’aggiunta di sensori angolari su ogni giunto che permettono di ricevere un feedback sulla reale posizione raggiunta dal manipolatore in modo da correggere errori introdotti dall’azione della forza di gravità e dall’eccessiva elasticità della struttura;
L’aggiunta di una camera sull’end-effector e di un monitor lato utente che permettano a quest’ultimo di essere guidato in qualche modo nell’operazione di presa a lunghe distanze;
Fornire al manipolatore la possibilità di muoversi aggiungendo delle ruote o una struttura cingolata;
Sostituire la tastiera con un joystick in modo da rendere più agevole il comando del robot.
The manipulator at work…
Altera InnovateItaly Design Contest 2011
01/12/2011
Real Time Control of an Anthropomorphic Robotic Arm using FPGA

18. Video Altera InnovateItaly Design Contest 2011 01/12/2011
Nel video saranno illustrati tutti i principali comandi implementati e saranno presentate alcune delle prese effettuate. Data la possibilità di impostare l’orientamento del polso da tastiera si può vedere come la operazioni di presa e posa vengono in qualche modo semplificate.
Altera InnovateItaly Design Contest 2011
01/12/2011
Real Time Control of an Anthropomorphic Robotic Arm using FPGA