Robot Kol İçin Ters Kinematik Sorunu

[ernuynk]

Merhaba arkadaşlar, uzun bir süredir bu konuyu nereye açsam diye araştırıyordum ki robotik sistem tasarım bölümünü gördüm. Kısaca projemden bahsedecek olursam; kinect kontrollü robot kol yapıyorum.Kullanmış olduğum robot kol aşağıdaki linkte verilmiştir.

Kullanılan Robot kol: http://www.upmatik.com/m/2016/04/30/robot1.jpg , 6 eksenli olarak bahsediliyor fakat gripper'ı açıp kapatan motor ve gripper'ı döndüren motorun kinematik denklemlerinde hesaba katılmadığını gördüm. Yani hesaplama için geriye 4 eksen kalmış oluyor. Fakat kinematik dersi almadığım ve uygulama yapmadığım için bunun nasıl yapılacağı hakkında bir bilgim yok. Processing ile programa denemeleri yaparken 2 eksen için yapılmış bir proje gördüm. Projede kullanılan hesaplama düzlemi şu şekilde; http://www.upmatik.com/m/2016/04/30/2.jpg , kodları incelediğimde omuz ve dirsek uygulaması yapılan bu projede Kosinüs teoreminin kullanıldığını gördüm. Fakat 2 den fazla eksende yapılan hesaplamalarda sanırım işin içine matrisle giriyor. 2 eksen için yapılan projede çalıştırdığım kod şu şekilde;

import SimpleOpenNI.*;
import processing.serial.*;

SimpleOpenNI kinect;
Serial port;

int segmentLength;
PVector shoulder;
PVector elbow;
PVector target;

void setup() 
{
  size(900*2, 800); 
  kinect = new SimpleOpenNI(this);
  kinect.enableDepth();
  kinect.enableRGB();
  kinect.enableUser();
  kinect.setMirror(true);
  
  segmentLength = 400;          //program içerisindeki çalışma uzayı
  shoulder = new PVector();
  elbow = new PVector();
  target = new PVector();
  shoulder.x = 0;
  shoulder.y = height/2;
  
  println(Serial.list());
  String portName = Serial.list()[0];
  port = new Serial(this, portName, 9600);
}

void draw() 
{
  background(255);
  kinect.update();
  image(kinect.depthImage(), 300, 100);
  image(kinect.rgbImage(), 1000, 100); 
  IntVector userList = new IntVector();
  kinect.getUsers(userList);
  
  if (userList.size() > 0)
    {
      int userId = userList.get(0);
        if ( kinect.isTrackingSkeleton(userId)) 
        {
          PVector rightHand = new PVector();
          kinect.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_HAND, rightHand);  //istenen eklem koordinatını alır
          kinect.convertRealWorldToProjective(rightHand, rightHand);  //çalışma uzayında istenen eklemi-koordinatı işliyor
          rightHand.y = rightHand.y + 100; 
          rightHand.x = rightHand.x + 300;          //sağ el için tam nokta belirleniyor
          fill(0, 255, 0);         //sağ el noktası için renk belirleniyor
          ellipse(rightHand.x, rightHand.y, 20, 20);    //sağ eldeki noktanın eksenleri ve boyutu
          target.x = rightHand.x;       
          target.y = rightHand.y;    //hesaplama ve hareketlerin yapılacağı hedef nokta eksenleri sağ elin eksenleri ile eşitleniyor
          
          // begin complex inverse kinematics math 
          PVector difference = PVector.sub(target, shoulder);
          float distance = difference.mag();  //logaritma hesaplaması yapılır
          
          // sides of the main triangle (ana üçgen kenarları)
          float a = segmentLength;
          float b = segmentLength;  
          float c = min(distance, segmentLength + segmentLength);  //en küçük değeri döndürür
          
          // angles of the main triangle (ana üçgen açıları)
          float B = acos((a*a + c*c - b*b)/(2*a*c));
          float C = acos((a*a + b*b - c*c)/(2*a*b));
          
         
          // C is also the elbow angle
          float D = atan2(difference.y, difference.x);
          
          // angle of the shoulder joint
          float E = D + B + C - PI; // Pi is 180 degrees in rad
          float F = D + B;
          
          // use SOHCAHTOA to find rise and run from angles (sin,cos ve tan açı hesaplamaları)
          elbow.x = (cos(E) * segmentLength) + shoulder.x;
          elbow.y = (sin(E) * segmentLength) + shoulder.y;
          target.x = (cos(F) * segmentLength) + elbow.x;
          target.y = (sin(F) * segmentLength) + elbow.y;
          
          // adjust angles based on orientation of hardware 
          float shoulderAngle = constrain(degrees(PI/2 - E),0, 180);  /*constrain; max ve min aşmaması için değer belirliyor 
                                                        (100 den aşağı düşmediği için omuz servosu düzgün çalışmıyor. Fakat PI/4 için biraz daha düzgün çalışıyor)*/
          float elbowAngle = degrees(PI - C);  
          
          fill(255, 0, 0);
          textSize(20);
          text("Omuz: " + int(shoulderAngle) +"\nDirsek: " + int(elbowAngle), 20, 20);// 6
          byte out[] = new byte[2];
          out[0] = byte(int(shoulderAngle));
          out[1] = byte(int(elbowAngle));
          port.write(out);
          fill(255, 0, 0);
          ellipse(shoulder.x, shoulder.y, 10, 10);
          ellipse(elbow.x, elbow.y, 8, 8);
          ellipse(target.x, target.y, 6, 6);
          stroke(0, 255, 0);
          strokeWeight(5);
          line(shoulder.x, shoulder.y, elbow.x, elbow.y);
          line(elbow.x, elbow.y, target.x, target.y);
        }
    }
}

void onNewUser(SimpleOpenNI curContext,int userId)
{
  println("onNewUser - userId: " + userId);
  println("\t İskelet Takibi Başlatıldı");
  
  kinect.startTrackingSkeleton(userId);
}

void onLostUser(SimpleOpenNI curContext,int userId)
{
  println("onLostUser - userId: " + userId);
}

void onVisibleUser(SimpleOpenNI curContext,int userId)
{
  //println("onVisibleUser - userId: " + userId);
}

Programın çalışması sonucunda, robot üzerindeki 2. ve 3. eksen yani omuz ve dirsek eklemleri düzgün ve bir şekilde çalışıyor. Fakat benim bu sisteme 4.ekseni yani bileği de ekleme gerek, önceliğim bu. Daha sonrasında 1.eksen için robotun sağa ve sola dönme hareketlerini inceleyeceğim. Sizce nereden başlamam gerek, nasıl bir yol izlemeliyim?

teşekkürler

[seyityildirim]

Öncelikle kolay gelsin.ben de bu işlerle uğraşmaya yeni başladım. Zafer Bingül 'ün Robot kinematiği kitabını tavsiye ile yeni aldım. Size de tavsiye ederim . Kinematik sorunlarınıza temel olur

[ernuynk]

Öncelikle kolay gelsin.ben de bu işlerle uğraşmaya yeni başladım. Zafer Bingül 'ün Robot kinematiği kitabını tavsiye ile yeni aldım. Size de tavsiye ederim . Kinematik sorunlarınıza temel olur

bende bu kitabı aldım fakat incelediğim kadarıyla, yukarıda da dediğim gibi 3 ve daha üzeri eksenler için matris anlatımı yapılmış. Tabi konu öyle olabilir bilmediğim için söylüyorum. Fakat processing veya c#da matrisler ile çözülmüş bir ters kinematik örneği bulamadığım için takıldım.

teşekkürler