Um robot que desenha

Xukyo

7 meses ago

O projeto aqui apresentado consiste em desenhar um braço robótico com um lápis numa folha de papel. Um ótimo exercício de programação e controlo de um braço robótico.

Hardware

Braço robótico prático
4x Servomotor SMS1012
Controlador de servomotor ou shield de servo
Fonte de alimentação de 5V
Cabo USB
Placa Arduino UNO

Objetivo

Objetivo: Desenhar um círculo utilizando um braço robótico

Para levar a cabo este projeto, precisamos de

Montagem do braço robótico (Estrutura)
Realizar a montagem eletrónica para controlar o braço e a alimentação eléctrica (Hardware)
Desenvolver o programa para controlar a sequência de servomotores de modo a descrever uma forma no espaço (Software)

Braço robótico

Objeto: A estrutura deve ser capaz de transportar um lápis e de se deslocar nas três dimensões do espaço.

O braço Handy está a ser utilizado para este projeto. Os motores escolhidos são micro-servomotores 9g leves, económicos e fáceis de utilizar.

O braço é montado com os seus servomotores e o seu suporte é fixado a uma placa. Para este projeto também desenhámos e imprimimos um suporte para lápis, que pode descarregar aqui.

Hardware

Objetivo: A eletrónica deve ser capaz de distribuir a alimentação de energia à placa de comando e aos servomotores e de acionar os servomotores através de um sinal PWM.

Arduino UNO

As placas Arduino são fáceis de utilizar, com um grande número de bibliotecas disponíveis e uma enorme comunidade de apoio. Como o programa requer um pouco de RAM, optámos pela placa Arduino UNO.

A placa arduino é alimentada pelo PC ou pelo cabo USB.

Acionamento do servomotor

A placa Arduino não consegue alimentar tantos servomotores, pelo que é necessário adicionar um controlador ao conjunto.

Um driver de servomotor foi construído seguindo este tutorial, e é alimentado por baterias em série.

Software

Objetivo du programme

Vamos escrever os objectivos do programa e dividi-los em tarefas a realizar

Descrever as coordenadas da forma no espaço
Transformar as coordenadas espaciais do círculo nos ângulos das articulações do robot.
Verificação da posição do robot
Converter os ângulos do robô em comandos do servomotor
Envio de um comando para o atuador

Descrever as coordenadas da forma no espaço

A forma escolhida é uma circunferência pela sua simplicidade. As equações paramétricas de uma circunferência no espaço são:

Para a coordenada z, consideramos o tamanho do lápis, ou seja, 20 mm.

Escrevemos uma função que devolve a posição alvo do robot Xd como uma função do passo de tempo:

void circleTraj(double R,double x,double y,double z,int i,double Te, float Xd[6]){
        Xd[0]=x+R*cos(2*PI*Te*i);
        Xd[1]=y+R*sin(2*PI*Te*i);
        Xd[2]=z;
        Xd[3]=0;
        Xd[4]=0;
        Xd[5]=0;
}

Transformar as coordenadas espaciais do círculo nos ângulos das articulações do robot.

O braço robótico pode ser descrito como uma cadeia robótica utilizando o método Denavit-Hartenberg. Este método é útil para controlar um braço com mais de três articulações.

Este método consiste em definir um sistema de coordenadas cartesianas para cada articulação, seguindo a mesma lógica, o que permite definir parâmetros para descrever matematicamente os movimentos de cada elo no espaço.

Definir os parâmetros para cada ligação robótica com base na geometria (parâmetro DH)

j	Sigmaj	Thetaj	Dj (mm)	Aj (mm)	alphaj
1	0	Th1	45	0	90
2	0	Th2	0	75	0
3	0	Th3	0	75	0
4	0	Th4	4	0	-90

O cálculo dos ângulos desejados do motor em função da posição desejada envolve a inversão da matriz Jacobiana definida com os parâmetros DH. Para os cálculos da matriz, utilizamos a biblioteca MatrixMath.h‘.

void computeAngle(){
    float J[6][nbLinks];
    float Jt[nbLinks][6];
    float JJt[nbLinks][nbLinks];
    float invJ[nbLinks][6];
    double detJ;
        //    compute jacobian
        jacobian(sig,a,al,d,th,q,nbLinks,(float*)J);

        // avoid singularity
        Matrix.Transpose((float*)J,6,nbLinks,(float*)Jt);
        Matrix.Multiply((float*)Jt, (float*)J, nbLinks, 6, nbLinks,(float*)JJt);

        
        detJ=detrm(JJt,nbLinks);
        // invert jacobian
        if(detJ>=1){
          Matrix.Invert((float*)JJt,nbLinks);
          Matrix.Multiply((float*)JJt,(float*)Jt,nbLinks,nbLinks,6,(float*)invJ);
        }
        
        for(int i=0;i<nbLinks;i++){
                qu[i]=0;
            for(int j=0;j<6;j++){
                qu[i]+=invJ[i][j]*Xu[j];
            }
        }
}

Função para converter os ângulos do robô em comandos para o servomotor

A conversão dos ângulos em controlo PWM depende do posicionamento dos servomotores, dos valores nominais de PWM e da convenção dos pontos de referência cartesianos utilizados para descrever as articulações do robô no espaço.

Servo/Joint	Min impulse	Max impulse	Min robot	Max robot
1	500	2500	-PI/2	PI/2
2	500	2500	PI	0
3	600	2600	-PI/2	PI/2
4	500	2500	-PI/2	PI/2

int jointToImp(double x,int i) {
        int imp=(x - joint_min[i]) * (imp_max[i]-imp_min[i]) / (joint_max[i]-joint_min[i]) + imp_min[i];
        imp=max(imp,imp_min[i]);
        imp=min(imp,imp_max[i]);
        return imp;  
}

Envio de um comando para o atuador

Para controlar os servomotores, usamos a biblioteca Servo.h disponível como padrão no IDE do Arduino. Escrevemos uma função que envia o comando PWM para cada servomotor de acordo com o ângulo solicitado.

void servoToImp(double q[nbLinks],int velocity){
  for (int i=0; i<nbLinks; i++) {
    servo[i].writeMicroseconds(jointToImp(q[i],i));
    delay(velocity);  
    }
}

Função de controlo do robô

Para controlar os servomotores, a posição desejada do robô deve ser sempre comparada com a sua posição atual.

Assim, calculamos a posição do robot em função dos ângulos do motor. Para o efeito, utilizamos as matrizes de transformação definidas pelo método de Denavit-Hartenberg.

        forwardKinematics(sig,a,al,d,th,q,nbLinks,X);

Aplicar o controlador para calcular a posição seguinte do robot

        circleTraj(20,165,0,20,k,Te,Xd);
        circleTraj(20,165,0,20,k+1,Te,Xk);
        for(int i=0;i<6;i++) Xu[i]=(Xk[i]-Xd[i]+gain*Te*(Xd[i]-X[i]));

Determinamos os ângulos do motor para atingir a posição seguinte

        for(int i=0;i<nbLinks;i++) qN1[i]=q[i];
        computeAngle();
        for(int i=0;i<nbLinks;i++) q[i]=qN1[i]+qu[i];

O controlo PWM é aplicado a cada servomotor

        servoToImp(q,10);

Código completo

Biblioteca personalizada de MatrixMath.h, MatrixMathExt.h

/*****************************************************************\
* Project : Handy
* Author : X. Wiedmer 
* Version : V01
* Date : 08/06/2017
* Revision : 
* Summary :
\*****************************************************************/
// Library
#include <SoftwareSerial.h>
#include <Servo.h>
#include <MatrixMathExt.h>

// Pin assignement
#define BASE 2 
#define SHOULDER 3 
#define ARM 4 
#define FOREARM 5
#define WRIST 6

//paramètres Denavit-Hartenberg
    const double baseLength = 45;//mm
    const double shoulderLength = 10;//mm
    const double armLength = 75;//mm
    const double forearmLength = 75;//mm
    const double wristLength = 40;//mm

    const int nbLinks=4;
    double sig[nbLinks]={0,0,0,0};
    double a[nbLinks]={0,armLength,forearmLength,wristLength};
    double al[nbLinks]={PI/2,0,0,0};
    double d[nbLinks]={baseLength,0,0,0};
    double th[nbLinks]={0,3*PI/4,-PI/2,-3*PI/4+PI/2};
    
    double restPos[nbLinks]={0,3*PI/4,-PI/2,-3*PI/4+PI/2};
    
    
//paramètres servomoteur
    const int servoPin[nbLinks]={BASE,SHOULDER,ARM,FOREARM};
    Servo servo[nbLinks];
    const double joint_min[nbLinks]={-PI/2,PI,-2*PI/3,-PI/2};
    const double joint_max[nbLinks]={PI/2,0,PI/2,PI/2};
    const int imp_min[nbLinks]={500,500,600,500};
    const int imp_max[nbLinks]={2500,2500,2600,2500};

//paramètre du contrôle
    double Te=0.01; // pas de temps du contrôleur
    double gain=30; // gain du contrôle
    int N=(int)2/Te+1; // nombre d'étape pour parcourir 2 fois le cercle

// Variables
    float X[6],Xd[6],Xk[6],Xu[6];
    double qN1[nbLinks],qu[nbLinks];
    double q[nbLinks]={0,3*PI/4,-PI/2,-3*PI/4+PI/2};

    int firstShot = 0; // Pour que le programme ne s'exécute qu'une fois


/******************************************************************\
* PRIVATE FUNCTION: setup
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Initiate inputs/outputs
*
\******************************************************************/
void setup(){
  Serial.begin(9600); // Initialise le port série

  for (int i=0; i<nbLinks; i++) { // Initiatlise et positionne les servomoteurs au démarrage
    servo[i].writeMicroseconds(jointToImp(0,i));
    servo[i].attach(servoPin[i]);
    delay(500);
  }
}

/******************************************************************\
* PRIVATE FUNCTION: loop
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Main Function of the code 
\******************************************************************/
void loop(){
  if (firstShot == 0){
    //follow trajectory
    for(int k=0;k<N;k++){

        //Calcule la position du robot
        forwardKinematics(sig,a,al,d,th,q,nbLinks,X);

        //Calcule la prochaine position
        circleTraj(20,165,0,20,k,Te,Xd);
        circleTraj(20,165,0,20,k+1,Te,Xk);
        for(int i=0;i<6;i++) Xu[i]=(Xk[i]-Xd[i]+gain*Te*(Xd[i]-X[i]));
        
        //Calcule les angles moteurs pour atteindre la prochaine position
        for(int i=0;i<nbLinks;i++) qN1[i]=q[i];
        computeAngle();
        for(int i=0;i<nbLinks;i++) q[i]=qN1[i]+qu[i];
        
        //Envoie la commande au servomoteur
        servoToImp(q,10);

        // Affiche les angles moteur à chaque itération
        Serial.print("-----------------Iteration ");
        Serial.println(k);
        Serial.print("angle base = ");
        Serial.println(q[0]);
        Serial.print("angle bras = ");
        Serial.println(q[1]);
        Serial.print("angle avant-bras = ");
        Serial.println(q[2]);
        Serial.print("angle poignet = ");
        Serial.println(q[3]);
    }
    firstShot=1;
  }
  if(firstShot==1){
    Serial.println("robot au repos");
    restR();
    Serial.println("robot stop");
    stopR();
    firstShot=2;
  }
  
  }

// Decrit la trajectoire dans l'espace en focntion du temps
void circleTraj(double R,double x,double y,double z,int i,double Te, float Xd[6]){
        Xd[0]=x+R*cos(2*PI*Te*i);
        Xd[1]=y+R*sin(2*PI*Te*i);
        Xd[2]=z;
        Xd[3]=0;
        Xd[4]=0;
        Xd[5]=0;
}

// Calcule la matrice de transformation d'un lien robotique
void computeLinkTransform(double a,double al,double d, double th, float T[][4]){
    float trans[4][4] = {{cos(th),  -sin(th)*cos(al),  sin(th)*sin(al), a*cos(th)},
          {sin(th),   cos(th)*cos(al), -cos(th)*sin(al), a*sin(th)},
          {     0,            sin(al),         cos(al),        d      },
          {     0,                 0,               0,            1      } };
    Matrix.Copy((float*)trans,4,4,(float*)T);
}

// Calcule la position de la chaine robotique
void forwardKinematics(double* sig,double* a,double* al,double* d,double* th,double* q, int nbLinks, float X[6]){
    float A[4][4]={{1, 0, 0, 0},
           {0, 1, 0, 0},
           {0, 0, 1, 0},
           {0, 0, 0, 1}};
    float R[4][4];
    float Ti[4][4];

    // Compute transformation matrix
    for (int i=0; i<nbLinks; i++){
        if (sig[i]==0){
            th[i]=q[i];
        }else{
            d[i]=q[i];
        }
        //A=A*Ti
        computeLinkTransform(a[i],al[i],d[i],th[i],Ti);
        Matrix.Multiply((float*)A, (float*)Ti,4, 4,4, (float*)R);
        Matrix.Copy((float*)R,4,4,(float*)A);
    }

    X[0]=A[0][3];
    X[1]=A[1][3];
    X[2]=A[2][3];
    X[3]=0;
    X[4]=0;
    X[5]=0;
}

// Calcule un produit vectoriel
void CrossProduct1D(float* a, float* b, float* r){
    /*Ex: double v1[3]={1,0,0};
    double v2[3]={0,0,1};
    double w[3];
    CrossProduct1D(v1,v2,w);*/

  r[0] = a[1]*b[2]-a[2]*b[1];
  r[1] = a[2]*b[0]-a[0]*b[2];
  r[2] = a[0]*b[1]-a[1]*b[0];

}

// Calcule la matrice jacobienne d'une chaine robotique
void jacobian(double* sig,double* a,double* al,double* d,double* th,double* q, int nbLinks, float* J){
    float Ti[4][4];
    float O[nbLinks+1][3];
    float z[nbLinks+1][3];
    float A[4][4]={{1, 0, 0, 0},
       {0, 1, 0, 0},
       {0, 0, 1, 0},
       {0, 0, 0, 1}};
    float R[4][4];
    float v[4];
    O[0][0]=0;
    O[0][1]=0;
    O[0][2]=0;
    
    z[0][0]=0;
    z[0][1]=0;
    z[0][2]=1;
    
    float v0[4]={0,0,1,1};
    
    for (int i=0; i<nbLinks; i++){
    
        // Compute transformation matrix
        if (sig[i]==0){
            th[i]=q[i];
        }else{
            d[i]=q[i];
        }
        computeLinkTransform(a[i],al[i],d[i],th[i],Ti);
        // A=A*T[j];
        Matrix.Multiply((float*)A, (float*)Ti,4, 4,4, (float*)R);
        Matrix.Copy((float*)R,4,4,(float*)A);
        //Compute joint center and orientation vector
        O[i+1][0]=A[0][3];
        O[i+1][1]=A[1][3];
        O[i+1][2]=A[2][3];
    
        Matrix.Multiply((float*)A, (float*)v0,4, 4,1, (float*)v);
        //z{i}=[v(1); v(2); v(3)]-O{i};
        z[i+1][0]=v[0]-O[i+1][0];
        z[i+1][1]=v[1]-O[i+1][1];
        z[i+1][2]=v[2]-O[i+1][2];
    
    }
    
    //Jacobian
    float tempv[3];
    float cprod[3];
    
    for(int i=0;i<nbLinks;i++){
        if (sig[i]==0){
            // Ji=[z(i-1)xON-O(i-1); z(i-1)]
            Matrix.Subtract((float*)O[nbLinks], (float*)O[i],1,3,(float*)tempv);
            CrossProduct1D(z[i],tempv,cprod);
            J[0*nbLinks+i]=cprod[0];
            J[1*nbLinks+i]=cprod[1];
            J[2*nbLinks+i]=cprod[2];
            J[3*nbLinks+i]=z[i][0];
            J[4*nbLinks+i]=z[i][1];
            J[5*nbLinks+i]=z[i][2];
        }else{
            J[0*nbLinks+i]=z[i][0];
            J[1*nbLinks+i]=z[i][1];
            J[2*nbLinks+i]=z[i][2];
            J[3*nbLinks+i]=0;
            J[4*nbLinks+i]=0;
            J[5*nbLinks+i]=0;
        }
    }
}

// Calcul le déterminant d'une matrice
float detrm(float a[nbLinks][nbLinks],float k)
{
  float s=1,det=0,b[nbLinks][nbLinks];
  int i,j,m,n,c;
  if(k==1)
  {
    return(a[0][0]);
  }
  else
  {
    det=0;
    for(c=0;c<k;c++)
    {
      m=0;
      n=0;
      for(i=0;i<k;i++)
      {
        for(j=0;j<k;j++)
        {
          b[i][j]=0;
          if(i!=0&&j!=c)
          {
            b[m][n]=a[i][j];
            if(n<(k-2))
              n++;
            else
            {
              n=0;
              m++;
            }
          }
        }
      }
      det=det+s*(a[0][c]*detrm(b,k-1));
      s=-1*s;
      }
  }
  return(det);
}
// Calcule les angles moteur en fonction de la position désirée
void computeAngle(){
    float J[6][nbLinks];
    float Jt[nbLinks][6];
    float JJt[nbLinks][nbLinks];
    float invJ[nbLinks][6];
    double detJ;
        //    compute jacobian
        jacobian(sig,a,al,d,th,q,nbLinks,(float*)J);

        // avoid singularity
        Matrix.Transpose((float*)J,6,nbLinks,(float*)Jt);
        Matrix.Multiply((float*)Jt, (float*)J, nbLinks, 6, nbLinks,(float*)JJt);

        
        detJ=detrm(JJt,nbLinks);
        // invert jacobian
        if(detJ>=1){
          Matrix.Invert((float*)JJt,nbLinks);
          Matrix.Multiply((float*)JJt,(float*)Jt,nbLinks,nbLinks,6,(float*)invJ);
        }
        
        for(int i=0;i<nbLinks;i++){
                qu[i]=0;
            for(int j=0;j<6;j++){
                qu[i]+=invJ[i][j]*Xu[j];
            }
        }
}

// Convertie l'angle moteur en commande PWM
int jointToImp(double x,int i) {
        int imp=(x - joint_min[i]) * (imp_max[i]-imp_min[i]) / (joint_max[i]-joint_min[i]) + imp_min[i];
        imp=max(imp,imp_min[i]);
        imp=min(imp,imp_max[i]);
        return imp;  
}

// Intitialise servomoteur
void servoAttach(){
  for (int i=0; i<nbLinks; i++) {
    servo[i].attach(servoPin[i]);
  }   
}

// Envoie la commande PWM à chaque servomoteur
void servoToImp(double q[nbLinks],int velocity){
  for (int i=0; i<nbLinks; i++) {
    servo[i].writeMicroseconds(jointToImp(q[i],i));
    delay(velocity);  
    }
}

// Met le robot en position de repos
void restR(){
  servoToImp(restPos,500);
}

// Déconnecte les servomoteurs
void stopR(){
  for(int j=0;j<nbLinks;j++){
    servo[j].detach();
  }
}

Resultados

O motor desenha na folha e segue a trajetória definida.

Se quiser ajuda com o seu braço robótico ou quiser mais informações sobre este projeto, deixe um comentário ou contacte-nos.

Hardware

Objetivo

Braço robótico

Hardware

Software

Objetivo du programme

Descrever as coordenadas da forma no espaço

Transformar as coordenadas espaciais do círculo nos ângulos das articulações do robot.

Função para converter os ângulos do robô em comandos para o servomotor

Envio de um comando para o atuador

Função de controlo do robô

Código completo

Resultados

Referência