서보모터 로봇팔에 Inverse Kinematics 함수만들어서 움직여보자

약간 어려운 아두이노편입니다.

뭐 그다지 코딩을 잘하는 것은 아니지만 새로운? 시도를 해보는 셈치고

이것저것 한번 해보고 있습니다.

먼저 하려고하는 것을 정의해두고 시작하겠습니다.

먼저 서보모터 1과 2로 구성된 관절과 일정한 길이의 막대기를 이용하여

로봇팔을 만듭니다.

일반적인 방식으로 제어를 하자면 서보모터1과 서보모터2의 각도를 조절해서

목표점(x,y)에 도달하게 하는 것입니다. 이게 Kinematics 방법입니다.

Inverse Kinematic란 입력값이 각 모터의 각도가 아니라

목표점을 입력으로 넣으면 서보모터1의 각도 세타1과

서보모터2의 각도 세타2가 산출되는 방식입니다.

그 각도에 따라서 서보모터를 움직여주면 원하는 목표점에

로봇팔의 끝이 도달하게 됩니다.

즉 제어변수가 각도가 아니라 목표점이 되겠습니다.

예를들어 원점에 위치한 로봇팔에다가

10cm앞에 2cm위에 놓은 물건에 도달하도록 입력하면

Input = (10,2)

세타1과 세타2가 뭔지는 모르겠지만 자동적으로 계산이되고

계산된값으로 모터를 회전시키면 되는 것입니다~

Inverse Kinematic에 대한 자세한 설명은 아래 링크로 대신합니다.

(여기서 수식도 그대로 가져와 아두이노 코드로 만들었습니다)

http://www.machinedesign.com/robotics/kinematics-and-dynamics-non-cartesian-actuators-and-robotics

(이미지 출처 : http://www.machinedesign.com/robotics/kinematics-and-dynamics-non-cartesian-actuators-and-robotics)

여기서 우리가 계산해내야 할것은 세타S와 세타E 입니다.

우선 본격적인 작업전에 서보모터 2개가 잘 작동하는지 테스트 해봤습니다.

잘 작동하네요 

테스트에 쓰인 서보모터는 SG-90과 SG-5010입니다

제가 쓴 서보모터는 3개 선으로 되어있습니다 Vcc, Gnd, Sig 입니다.

선의 칼라는 SG 시리즈는 비슷비슷한가 봅니다.

빨간선-Vcc

검정선(갈색선)-Gnd

오렌지선-Sig

위 선색깔을 참조해서 아두이노에 연결해줬습니다.

D.9는 아두이노 디지털9번핀 D.10은 디지털10번핀입니다.

다음으로 로봇팔처럼 간단하게 한번 만들어보려고 나무젓가락 2개를 준비했습니다.

나무젓가락은 대략 20센치정도 되네요

조금더 튼튼해보이는? SG-5010에 나무젓가락 1개를 우선 글루건으로 붙여줬습니다.

그리고 끝에 SG-90을 붙여줬습니다.

SG-90도 마찬가지로 나무젓가락 1개를 붙여줬습니다.

믿기지 않겠지만 이게 로봇팔입니다.

다음으로 묵직한 상자를 하나 준비해서 

5센치정도 여유를 두고 로봇팔을 붙여줬습니다.

대략 이런모습이네요~

다음으로 제가 만든 베이직한 코드를 한번 업로드 해봣습니다.

#include <Servo.h> Servo myservo;  // create servo object to control a servo(SG-5010) Servo myservo2;  // create servo object to control a servo(SG-90) float L1 = 18; //첫번째 서보모터에서 두번째 서보모터 사이 막대기의 길이 float L2 = 18.5; //두번째 서버모터에 달린 막대기의 길이 float X = 18; //초기 목표점의 X좌표 float Y = 18.5; //초기 목표점의 Y좌표 float thetaE = 0; //각도 출력을 위한 변수(OUTPUT) float thetaQ = 0; //각도 출력을 위한 변수(OUTPUT) float thetaS = 0; //각도 출력을 위한 변수(OUTPUT) void setup() {   // put your setup code here, to run once:   Serial.begin(9600); //시리얼포트 오픈   myservo.attach(9);  // attaches the servo on pin 9 to the servo object   myservo2.attach(10);  // attaches the servo on pin 10 to the servo object   thetaE = acos((pow(X,2)+pow(Y,2)-pow(L1,2)-pow(L2,2))/(2*L1*L2));   //....①   thetaQ = acos((pow(X,2)+pow(Y,2)+pow(L1,2)-pow(L2,2))/(2*L1*(sqrt((pow(X,2)+pow(Y,2))))));  //....②   thetaS = atan2(Y,X) - thetaQ; //....④   Serial.print(thetaE * 180 / PI); //값 확인을 위한 출력   Serial.print(","); //값 확인을 위한 출력   Serial.println(thetaS * 180 / PI); //값 확인을 위한 출력   myservo.write(thetaS * 180 / PI); //acos,atan 등 함수의 리턴값이 라디안 단위고 서보모터에서 사용하는 단위는 디그리단위라서   myservo2.write(thetaE * 180 / PI); // 180을 곱하고 π로 나눠서 라디안을 디그리로 바꿔서 각도입력 } void loop() {   // put your main code here, to run repeatedly:    }

그냥 입력해놓은 좌표로 아두이노가 켜지면 움직이게 하는 코드입니다.

여기서 ①은 저기 위에 세타E를 구하는 공식을 그대로 C코드로 표현한것입니다.

②도 마찬가지로 C코드로 표현한것입니다.

atan2 함수도 아두이노에 그냥 있기때문에 그대로 적용가능햇습니다.

위 코드에서 L1은 꼭지점인 서보모터1에서 두번째 관절인 서보모터2까지 붙인

젓가락의 길이를 뜻합니다. 저는 18센치네요~

마찬가지로 L2는 서보모터에 달린 젓가락의 길이로 18.5 센치입니다.

위코드를 실행하면 (x,y) = (18,18.5)인 점에 젓가락 끝이 위치하려면

각이 몇도여야 하겠는가? 라고 아두이노한테 물어보는것과 같습니다.

직관적으로 생각하기에 첫번째 젓가락이 18센치고 두번째 젓가락이 18.5센치니

서보모터2가 정확히 90도로 돌아가면 해당점에 위치 시킬 수 있습니다.

여기서 한스탭더 나아가서 시리얼통신으로 좌표를 입력하면

입력한 좌표대로 서버모터가 작동하게 한번 만들어 봤습니다.

#include <Servo.h> Servo myservo;  // create servo object to control a servo Servo myservo2;  // create servo object to control a servo float L1 = 18; float L2 = 18.5; int X = (int)(L1+L2); int Y = 0; float thetaE = 0; float thetaQ = 0; float thetaS = 0; void setup() {   // put your setup code here, to run once:   Serial.begin(9600);   myservo.attach(9);  // attaches the servo on pin 9 to the servo object   myservo2.attach(10);  // attaches the servo on pin 9 to the servo obje            thetaE = acos((pow(X,2)+pow(Y,2)-pow(L1,2)-pow(L2,2))/(2*L1*L2));         thetaQ = acos((pow(X,2)+pow(Y,2)+pow(L1,2)-pow(L2,2))/(2*L1*(sqrt((pow(X,2)+pow(Y,2))))));         thetaS = atan2(Y,X) - thetaQ;         Serial.print(thetaE * 180 / PI);         Serial.print(",");         Serial.println(thetaS * 180 / PI);         myservo.write(thetaS * 180 / PI);              // tell servo to go to position in variable 'pos'         myservo2.write(thetaE * 180 / PI);              // tell servo to go to position in variable 'pos'         Serial.println("SETUP"); } void loop() {   // put your main code here, to run repeatedly:     while (Serial.available() > 0) {     X = Serial.parseInt();     Y = Serial.parseInt();         float X_max = (L1+L2);       float X_min = (L1-L2) * cos(PI/4);       float Y_max = (L1+L2));       float Y_min = (L1-L2) * sin(PI/4);       X = constrain(X,X_min,X_max);       Y = constrain(Y,Y_min,Y_max);           if (Serial.read() == '\n') {         thetaE = acos((pow(X,2)+pow(Y,2)-pow(L1,2)-pow(L2,2))/(2*L1*L2));         thetaQ = acos((pow(X,2)+pow(Y,2)+pow(L1,2)-pow(L2,2))/(2*L1*(sqrt((pow(X,2)+pow(Y,2))))));         thetaS = atan2(Y,X) - thetaQ;                  Serial.print(thetaE * 180 / PI);         Serial.print(",");         Serial.println(thetaS * 180 / PI);         myservo.write(thetaS * 180 / PI);              // tell servo to go to position in variable 'pos'         myservo2.write(thetaE * 180 / PI);              // tell servo to go to position in variable 'pos'       }     } }

이전 코드와 거의 비슷하지만 시리얼통신으로 숫자 2개를 입력받아서(그게 좌표가 되겠죠)

그 값대로 각을 돌리는 예제를 만들었습니다.

중요한건 빨간색 부분만 보시면 됩니다.

int X = (int)(L1+L2);

int Y = 0;

이건 아두이노를 켰을때 서보모터의 상태를 뜻합니다.

이 코드 이전에 서보모터의 길이를 측정해서 L1과 L2로 입력을 해뒀을텐데

(저의 경우 18센치와 18.5센치)

x가 L1+L2고 y가 0이란 소리는

첫번째 서보모터와 두번째 서보모터가 모두 0도여아 가능합니다.

왜냐하면 두개의 젓가락을 모두 합한길이니 젓가락 끝이 해당지점에 도달하기위해서는

로봇팔을 최대한 편상태여야 가능하기 때문이죠

여기서 중요힌 한트를 하나 얻습니다.

젓가락 길이를 잘 생각해보면 절대로 도달할 수 없는 좌표들이 있습니다.

사용자가 입력하게될 x좌표와 y좌표의 범위가 생긴다는 의미인데

최소 몇에서 최대 몇까지 입력 가능한지 생각을 해봤습니다.

      float X_max = (L1+L2);

      float X_min = (L1-L2) * cos(PI/4);

      float Y_max = (L1+L2);

      float Y_min = (L1-L2) * sin(PI/4);

      X = constrain(X,X_min,X_max);

      Y = constrain(Y,Y_min,Y_max);

그냥 간단하게 생각해봤습니다

Y가 0인채로 로봇팔을 최대한 뻗을 수 있는 X좌표가 L1+L2입니다.

마찬가지로 X가 0인상태로 로봇팔을 위로 최대한 뻗을 수 있는 Y좌표도 L1+L2입니다.

가장 최소로 입력할 수 있는 값은

첫번째 서보모터와 두번째 서보모터가 45도인채로 겹쳐졌을때 인것 같습니다.

(제 생각에는...)

이렇게 해서 사용자가 입력값을 초과하면 자동적으로

최대값을 지정해서 움직일 수 있게 코드를 짜봤습니다.

시리얼 입력으로 x=5cm y=5cm를 입력해봤습니다.

두번째 서보모터만 작동을해서 해당 위치를 맞추고 있는 모습입니다.

서버모터 자체가 저가형이라서 정확하지는 않습니다.

다음으로 x=10, y=10을 입력해봤습니다.

약간 더 위로 움직이네요~

작동은 잘 되는것 같습니다.

이제 세번째 스텝인 조이스틱 구동으로 한번 넘어가봤습니다.

조이스틱은 이걸로 했습니다.

선 5개 꽂아놨는데 실제로는 4개만 쓸거에요

GND, VCC, X, Y 이렇게 있습니다.

VCC와 GND는 아두이노에 5V와 GND에 각각 연결하고

X는 아날로그 0번포트

Y는 아날로그 1번포트에 연결해줬습니다.

대략 이런 모양으로 한번 만들어 봤습니다.

업로드할 코드를 한번 살펴보죠

#include <Servo.h> Servo myservo;  // create servo object to control a servo Servo myservo2;  // create servo object to control a servo float L1 = 18; float L2 = 18.5; int X = (int)(L1+L2); int Y = 0; int Joy_X = 0; int Joy_Y = 0; float X_max = (L1+L2); float X_min = (L1-L2) * cos(PI/4); float Y_max = (L1+L2); float Y_min = (L1-L2) * sin(PI/4); int pre_X = constrain(X,X_min,X_max);; int pre_Y = constrain(Y,Y_min,Y_max);; float thetaE = 0; float thetaQ = 0; float thetaS = 0; void setup() {   // put your setup code here, to run once:   Serial.begin(9600);   myservo.attach(9);  // attaches the servo on pin 9 to the servo object   myservo2.attach(10);  // attaches the servo on pin 9 to the servo obje            thetaE = acos((pow(X,2)+pow(Y,2)-pow(L1,2)-pow(L2,2))/(2*L1*L2));         thetaQ = acos((pow(X,2)+pow(Y,2)+pow(L1,2)-pow(L2,2))/(2*L1*(sqrt((pow(X,2)+pow(Y,2))))));         thetaS = atan2(Y,X) - thetaQ;         Serial.print(thetaE * 180 / PI);         Serial.print(",");         Serial.println(thetaS * 180 / PI);         myservo.write(thetaS * 180 / PI);              // tell servo to go to position in variable 'pos'         myservo2.write(thetaE * 180 / PI);              // tell servo to go to position in variable 'pos'         Serial.println("SETUP"); } void loop() {      Joy_X = analogRead(A0);   Joy_Y = analogRead(A1);   if(Joy_X > 800) X++;   else if(Joy_X < 300) X--;   if(Joy_Y > 800) Y--;   else if(Joy_Y < 300) Y++;          X = constrain(X,X_min,X_max);       Y = constrain(Y,Y_min,Y_max);          if(X != pre_X || Y != pre_Y){         thetaE = acos((pow(X,2)+pow(Y,2)-pow(L1,2)-pow(L2,2))/(2*L1*L2));         thetaQ = acos((pow(X,2)+pow(Y,2)+pow(L1,2)-pow(L2,2))/(2*L1*(sqrt((pow(X,2)+pow(Y,2))))));         thetaS = atan2(Y,X) - thetaQ;                  Serial.print(thetaE * 180 / PI);         Serial.print(",");         Serial.println(thetaS * 180 / PI);         myservo.write(thetaS * 180 / PI);              // tell servo to go to position in variable 'pos'         myservo2.write(thetaE * 180 / PI);              // tell servo to go to position in variable 'pos'      }      pre_X = X;      pre_Y = Y; }

빨간색 부분이 변경된 부분입니다.

먼저 조이스틱 입력을 받기위해서 변수 2개를 선언해주고

조이스틱 상태에 따라서 0~1023값이 들어올겁니다.

대략해보니 중간에 있을때 600이 입력되고

완전 꺾으면 100이하거나 1000이상이 찍히네요

그래서 조건을 위와같이 800과 300으로 잡았습니다.

조이스틱 꺾은거에 따라서 목표점이 바뀌는 방식입니다.

그리고 불필요한 연산을 줄이기 위해서 조건을 하나더 넣어놨습니다.

값의 변경이 있을때만 계산을해서 각도에 반영하도록 만들었네요

왜냐하면 조이스틱이 가만히 있는데 계산을해봤자

똑같은 값이 나올거기 때문에 적용해둔 코드입니다.

구동영상은 아래 넣어둡니다.

이상으로 inverse kinemetics편 마치겠습니다.

허접한글 읽어주셔서 감사합니다.