Programa un robot para dibujar

Xukyo

hace 6 años

Un buen ejercicio de programación es programar un brazo robótico para que reproduzca una trayectoria, como dibujar en una hoja de papel. Una vez que tenga este conocimiento, se puede programarlo para hacer lo que quiera.

Material

Brazo robotico Handy
4x Servomotor SMS1012
Servocontrolador
5V alimentación
Cable USB
Arduino UNO

Objectivo

Para realizar este proyecto, necesitamos:

Montar el brazo robótico (Estructura)
Enlace los componentes electrónicos al motor y la potencia para permitir el control del brazo (Hardware)
Escriba el programa para que el robot siga una trayectoria (Software)

Estructura

Obj: la estructura debería poder llevar un bolígrafo en tres dimensiones de espacio.

Usamos un brazo robótico de escritorio Handy para este proyecto. Las partes están optimizadas para un pequeño servomotor y se imprimieron desde la base hasta la muñeca. Se diseñó e imprimió un bolígrafo adicional.
que puedes descargar aquí El brazo está montado con 4 micro servomotores y la base está unida a una placa de madera.

Hardware

Obj: El microcontrolador debe estar conectada al servomotor y a la alimentación para controlar el servomotor con una señal PWM.

Arduino UNO

Arduino, son fáciles de usar y hay una gran comunidad para pedir ayuda. Como el programa pide un poco de memoria, elegimos un Arduino UNO.

Servocontrolador

El Arduino no puede alimentar tantos servomotores. Se debe agregar un controlador para distribuir la energía desde otra fuente.

Este tutorial fue seguido para hacer el controlador.

Software

Objectivos

El objetivo del programa se puede dividir en tareas más pequeñas:

Describe las coordenadas de la trayectoria en el espacio
Transforme las coordinadas de la trayectoria en los ángulos deseados de las juntas del robot.
Controla la posición del robot
Convertir ángulos de robot en valor de comando del servomotor
Enviar comando a servomotor

Describe las coordenadas de la trayectoria en el espacio

La trayectoria elegida es un círculo por su simplicidad. Las ecuaciones paramétricas son:

Para las coordenadas z, tomamos la altura del lápiz con respecto a la muñeca: 20 mm.

Escribimos una función que genera la posición deseada con respecto al paso de tiempo:

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;
}

Transforme las coordinadas de la trayectoria en los ángulos deseados de las articulaciones del robot

El brazo robótico se puede describir con el sistema Denavit-Hartenberg como una cadena robótica con 4 enlaces articulados. Este método es interesante para controlar proyectos articulados con más de 3 articulaciones.

El método define un marco cartesiano para cada unión siguiendo la misma lógica que da la definición de parámetros que pueden usarse para describir matemáticamente el movimiento de los enlaces.

Definición de los parámetros con respecto a la geometría (parámetros 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

Para calcular los ángulos deseados a partir de la posición deseada, usamos el inverso de la matriz jacobiana definida con los parámetros denavit-hartenberg. Para calcular matrices usamos la 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];
            }
        }
}

Convierta el ángulo de articulación en el comando del servomotor

Para convertir ángulos en comandos PWM, se debe conocer la posición del servomotor con respecto a los valores nominales de unión y PWM. Esta conversión también depende de los marcos cartesianos utilizados para describir el enlace mecánico con el método DH.

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;  
}

Enviar comando a servomotor

Para mover los servomotores, usamos la biblioteca Servo.h disponible en el IDE de Arduino. Escribimos una función que envía el comando PWM a cada servomotor de acuerdo con el á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);  
    }
}

Controla la posición del robot

Para controlar los servomotores, la posición deseada y real se debe comparar en tiempo real

La posición del robot se calcula con el ángulo de las articulaciones utilizando las matrices de transformación definidas con el método denavit-hartenberg.

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

Controle se aplica para calcular la siguiente posición deseada

        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]));

Los ángulos del servomotor deseados se calculan para llegar a la siguiente posición deseada

        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];

El comando PWM se aplica a todos los servos.

        servoToImp(q,10);

Código completo

la biblioteca cambió 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();
  }
}

Resultado

El robot sigue la trayectoria y puede dibujar un círculo en el papel

Si desea obtener más información sobre este proyecto, no dude en dejar un comentario o envíenos un mensaje.

Fuentes

Puede encontrar este robot aqui.

Handy