Delta Robot 기구학 해석2

안녕하세요. 

 

오늘은 저번 포스팅에 이어 델타로봇 기구학 해석을 이어 포스팅 하겠습니다.

 

지난번 포스팅에서 처럼 아래의 함수 형태로 위치 좌표값과 3개의 모터중 하나를 선택할 수 있는 형태 입니다.

 

deltakinematic(float posX, float posY, float posZ, char servo);

 

그렇게 되면 함수에서 각 3개의 모터의 각도값을 산출하게 됩니다.

 

현재 한 축에 대한 각 기구물의 제원은 아래와 같습니다.

 

먼저 기본이 되는 한 축에 대해 기구학을 적용하여 풀이를 합니다.

 

이후 나머지 두개의 축은 120도의 각도 간격으로 배치 되어 있으므로 수식으로 유도 합니다.

 

단위는 mm 입니다.

 

 

이 자료는 원본이 있는 자료 입니다.

 

 출처: github.com/deltarobotone/how_to_build_your_robot/wiki/Kinematics

deltarobotone/how_to_build_your_robot

 

좌표의 움직임은 아래와 같습니다.

 

 

결론부터 이야기하면 저런 좌표로 움직이게 하기위해 모터3개를 각도 조정 하는것 입니다.

 

그 마지막 각도를 구하기 위해 약간 간단 복잡한 기구학적 수식을 유도 합니다.

 

 

위의 그림처럼 최종각도 C를 구하는 것이 목표 입니다. 각도 C가 모터가 움직이는 각도 입니다.

그러기 위해 나머지 각도 A,B를 먼저 유도 합니다.

 

length.a = 31 (기구물 제원: 위쪽그림 참고)

length.d = 15 (기구물 제원: 위쪽그림 참고)

 

y = y축으로 움직인 거리

z = z축으로 움직인 거리

 

1. 각도 A 유도식

float length1 = (length.a - length.d - y);

float A = (360.0F / (2.0F * PI)) * (atan2(z, length1));

 

 

피타고라스 삼각함수를 사용하여 아래식을 유도 합니다.

 

float length2 = sqrt(pow(length1, 2.0F) + pow(z, 2.0F));

float lenght3 = sqrt(pow(length.c, 2.0F) - pow(x, 2.0F));

 

다음으로 COSINE 삼각함수를 이용하여 각도 B를 구합니다.

float B = (360.0F / (2.0F * PI)) * (acos((pow(lenght3, 2) - pow(length2, 2.0F) - pow(length.b, 2.0F)) / (-2.0F * length2 * length.b)));

 

최종적으로 각도 C를 아래의 수식으로 유도 합니다.

 

float C= 180.0F - A - B;

 

이제껏 기본 한 축에 대한 수식 유도를 한 것이고 기구학적으로 3개의 축이 120도의 각도로 배치되어 있습니다.

기본적으로 모터 A를 기본으로 한 나머지 모터B, 모터C에 대한 위치 수식은 아래와 같습니다.

 

if (servo == 'A')

{

  x = posX;

  y = posY;

  z = posZ;

}

if (servo == 'B')

{

  x = (cos(pi120)*(posX)) + (sin(pi120)*(posY));

  y = -(sin(pi120)*(posX)) + (cos(pi120)*(posY));

  z = posZ;

}

if (servo == 'C')

{

  x = (cos(pi240)*(posX)) + (sin(pi240)*(posY));

  y = -(sin(pi240)*(posX)) + (cos(pi240)*(posY));

  z = posZ;

}

 

기구학 수식을 적용한 최종 코드는 아래와 같습니다.

 

float DeltaRobot::Move::deltakinematic(float posX, float posY, float posZ, char servo)

{

 float x = 0.0F, y = 0.0F, z = 0.0F;

 float pi120 = 120.0F * (PI / 180.0F);

 float pi240 = 240.0F * (PI / 180.0F);

 if (servo == 'A')

 {

  x = posX; y = posY; z = posZ;

 }

 if (servo == 'B')

 {

  x = (cos(pi120)*(posX)) + (sin(pi120)*(posY));

  y = -(sin(pi120)*(posX)) + (cos(pi120)*(posY));

  z = posZ;

 }

 if (servo == 'C')

 {

  x = (cos(pi240)*(posX)) + (sin(pi240)*(posY));

  y = -(sin(pi240)*(posX)) + (cos(pi240)*(posY));

  z = posZ;

 }

 

 float length1 = (length.a - length.d - y);

 float A= (360.0F / (2.0F * PI)) * (atan2(z, length1));

 float length2 = sqrt(pow(length1, 2.0F) + pow(z, 2.0F));

 float lenght3 = sqrt(pow(length.c, 2.0F) - pow(x, 2.0F));

 float B= (360.0F / (2.0F * PI)) * (acos((pow(lenght3, 2) - pow(length2, 2.0F) - pow(length.b, 2.0F)) / (-2.0F * length2 * length.b)));

 float C= 180.0F - A- B;

 return C;

 

}

 

이 하나의 수식으로 위치값을 넣어서 모터3개의 각도를 유도 할 수 있습니다.

 

제어주기를 빠르게 두어 각 모터를 순차적으로 하나씩 각도를 유도 해냅니다.

 

다음 포스팅은 실제 코드에 적용하여 동작시키는 과정을 포스팅하겠습니다.

 

도움이 되셨다면 광고 클릭 부탁드립니다.