2016년 3월 6일 일요일

아두이노 기반 6 DoF 로봇암 조립 방법 및 제어

오늘은 6축의 자유도를 가진 6 DoF (자유도) Robot Arm Kit 조립 및 개발 방법을 정리해 본다. 6축 자유도는 사람의 팔 구조와 매우 유사한 동작을 할 수 있다.


사용할 로봇암은 Oak Studio 6 DoF 이다. 여기에 사용된 MG 996R 서보 모터 제원은 다음과 같다.

Weight- 55g 
Dimension 40.7*19.7*42.9mm 
Stall torque 10kg/cm 
Operating speed 0.20sec/60degree(4.8v) 
Operating voltage 4.8-7.2V 
Temperature range 0_ 55  
Servo Plug: JR (Fits JR and Futaba)


이 모터는 토크가 그리 크지 않아, 큰 부하를 처리하지는 못한다. 이와 관련해, 입력 전압이 최대 7.2볼트라 토크에 한계가 있다. 그러므로, 가벼운 부하를 다루는 데 적합하다. 참고로, 큰 부하를 모터에 가하면, 아두이노 보드에 인터럽트가 발생하여, 모든 동작이 정지하게 된다.

이와 관련된 제작 방법은 다음과 같다.

1. 로봇암 조립

로봇암의 부품은 매우 많다. 그러므로, 관절에 해당하는 서보 모터 회전 각도 등을 고려해 순서대로 잘 생각한 후 결속한다.

특히, 브라켓(bracket)과 서보 모터를 결속은, 회전 각도가 잘못되어 있을 경우, 다시 결속된 것을 해체하고 회전 각도를 조정한 후, 다시 결속해야 하므로 주의한다. 

다음은 결속한 순서대로 표시한 그림이다. 






다음은 완성된 모습이다. 


이렇게 작업할 때 걸린 시간은 대략 3~4시간 정도이다. 우선 결속해야 할 브라켓, 나사와 너트가 너무 많다. 또한, 잘못 결속했을 때 몇번의 시행착오도 종종 발생한다.

사실 아직 마무리 못한 것이 있다. 바닥 회전판의 서보 모터 각도를 잘 못 맞추어 다시 나사 풀고 결속해야 한다. 실수할 때마다, 이웃 브라켓에 연결된 여덜개의 나사와 너트도 다시 풀고, 결속해야 하는 삽질을 계속해야 한다. 그러니, 잘 생각해서 작업하자.

머리 나쁘면 삽질이다

2. 로봇암 제어
이제 로봇암을 제어해 보도록 하자. 이 부분은 다음 레퍼런스를 참고하였다. 


우선 아두이노 보드 입력 핀과 각 서보모터의 신호, GND, VCC핀을 연결한다. 서보의 VCC는 7.2볼트 배터리에 연결한다. 참고로, 아두이노 5볼트에 연결하면, 전압이 약해, 제대로 동작하지 않는다. 

서보 신호 핀과 연결은 다음과 같다. 

바닥 서보: 3번 핀
어깨 서보: 5번 핀 
윗팔 서보: 6번 핀
손목 굽힘 서보: 9번 핀
손목 회전 서보: 10번 핀
손 그립(grip) 서보: 11번 핀

결선 후, 다음과 같이 코딩한다. 단, 각 관절 모터의 최대, 최소 각도는 조립전에 확인하여, 제한값을 두도록 한다. 그렇지 않으면, 프레임 간에 간섭이 생겨, 프레임이나 모터가 망가질 수 있다. 

#include <Servo.h>

#define NO_OF_FIELDS 6

Servo base, shoulder, foreArm, wristFlex, wristRotate, claw;

int inCommand[NO_OF_FIELDS];
int ledPin = 13;

void EchoData(int echoWord[])
{
 Serial.print("*");
 float total;
 for (int i = 0; i < NO_OF_FIELDS; i++)
 {
  Serial.print((int)echoWord[i]); 
  total+= inCommand[i];
  Serial.print(",");
 }
 Serial.print((int)total);
 Serial.print(",");
 Serial.print("*");
 Serial.print("&");
}

void ClearOldData(int oldData[])
{
 for (int i = 0; i < NO_OF_FIELDS; i++)
 {
  oldData[i] = 0;
 }
}

void setup()
{
 Serial. begin(38400);

 base.attach(3);
 shoulder.attach(5);
 foreArm.attach(6);
 wristFlex.attach(9);
 wristRotate.attach(10);
 claw.attach(11);

 //initialise all arms 
 base.write(20);      // 시계반대방향. 0 - 100, 20
 shoulder.write(30);  // 30 - 180, 30
 foreArm.write(10);   // 0 - 100, 10
 wristFlex.write(40); // 0 - 150, 40
 wristRotate.write(0);  // 0 - 180, 0
 claw.write(130);       // 90 - 140,  

 pinMode(ledPin, OUTPUT);
}

void demo()
{
 //initialise all arms
 for(int i = 20; i < 70; i += 1)
 {
   base.write(i);      // 시계반대방향. 0 - 100, 20
   delay(50);
 }
 for(int j = 70; j > 20; j -= 1)
 {
   base.write(j);      // 시계반대방향. 0 - 100, 20
   delay(50); 
 }

 for(int i = 30; i < 70; i += 1)
 {
   shoulder.write(i);  // 30 - 180, 30
   delay(50);
 }
 for(int j = 70; j > 30; j -= 1)
 {
   shoulder.write(j);  // 30 - 180, 30
   delay(50);  
 }

 foreArm.write(10);   // 0 - 100, 10
 wristFlex.write(40); // 0 - 150, 40
 wristRotate.write(0);  // 0 - 180, 0

 for(int i = 90; i < 150; i += 1)
 { 
   claw.write(i);       // 90 - 140,    
   delay(50);
 }
 for(int j = 150; j > 90; j -= 1)
 {
   claw.write(j);       // 90 - 140,    
   delay(50);
 }
}

void control()
{
 if(Serial.available())
 {
  for(int i = 0; i < NO_OF_FIELDS; i++)
  {
   inCommand[i] = Serial.parseFloat();
  }

  unsigned long time1 = millis();

  while (Serial.read() != -1)
  {
   //loops here until serial buffer becomes empty
   //Serial.println(millis() - time1);
   digitalWrite(ledPin, HIGH);
  }

  digitalWrite(ledPin,LOW); 

  //echo the incoming command sequence
  EchoData(inCommand);

  //now do something with the inCommand[] array.... 
  base.write(inCommand[0]);
  shoulder.write(inCommand[1]);
  foreArm.write(inCommand[2]);
  wristFlex.write(180 - inCommand[3]);
  wristRotate.write(inCommand[4]);
  claw.write(inCommand[5]);

  //clear the inCommand[] array
  ClearOldData(inCommand);
 }
}

void loop()
{  
  demo();
}

현재는 demo() 함수가 호출되고, 제대로 하였다면, 다음과 같이 로봇암이 제어되는 것을 확인할 수 있을 것이다. 



이제 시리얼 모니터(Serial monitor)에서 38400 bps로 속도를 맞춰 놓고, 6개의 각도에 대한 숫자를 다음과 같이 입력해 본다. 

20.0 30.0 10.0 40.0 0.0 130.0

제대로 입력되면, 정상적으로 로봇암이 해당 각도대로 제어될 것이다. 

3. 마무리
6 DoF 로봇암은 사람 팔과 자유도가 비슷한다. 그러므로, 로봇암을 마운팅한 로버 등을 이용하면, 탐사나 인스펙션 중에 장애물을 제거하거나, 스위치를 조작하거나, 개체를 획득하기 쉽다.

다만, 테스트해 본 결과로는 여기서 사용하는 6 DoF robot arm은 매우 가벼운 장애물만 제거하거나 옮길 수 있었다. 무게가 있는 장애물은 처리하기가 곤란하기 때문에, 이 경우에는 좀 더 높은 토크를 지원하는 서보를 사용해야 한다.

댓글 64개:

  1. 어떻게 아두이노 한개로 서보모터 6개를 제어하셨나요?

    답글삭제
    답글
    1. 아두이노 메가 사용하거나 모터 드라이버 보드 사용하면 제어할수 잇습니다. 모터 드라이버는 아래 링크를 참고하시길 바랍니다

      https://www.adafruit.com/product/815

      삭제
  2. can u provide me with the DH parameters of the arm if u have them?
    i want to use it for the Forward and inverse kinematics
    thanks

    답글삭제
    답글
    1. Hi. I don't remember the DH parameters. In addition, if you visit the below link, you can find the frame dimensions.

      https://www.aliexpress.com/item/The-latest-The-chassis-is-more-heavy-6-dof-robot-arm-mechanical-claw-6PCS-MG996R-High/2025394842.html?spm=2114.search0104.3.1.VpVvPJ&ws_ab_test=searchweb0_0,searchweb201602_4_10152_10065_10151_10068_10344_10342_5460018_10343_10340_10341_5470018_10307_10137_10060_10155_10154_5590018_10056_10055_10054_10059_10532_100031_10099_10338_10339_5580018_10103_10102_10052_10053_10142_10107_10050_10051_5370018_10326_10084_10083_10080_10082_10081_10110_10175_10111_10112_10113_10114_5380018_143_10312_10313_10314_10078_10079_10073,searchweb201603_30,ppcSwitch_4&btsid=f5617fd7-9352-4f71-9be4-3461cfb8ec79&algo_expid=a3364296-6e63-4d76-a88d-441faa8e070b-0&algo_pvid=a3364296-6e63-4d76-a88d-441faa8e070b

      삭제
  3. 작성자가 댓글을 삭제했습니다.

    답글삭제
    답글
    1. 올려 놓은 코드는 서보를 구동시키는 것에만 관련된 소스입니다.
      OpenCV로 사물을 인식하는 소스는 예제로도 나와 있는 데, 카메라 상의 사물 위치를 확인하고, PID 제어를 통해, 위치를 카메라 중심과 사물 중심이 일치하도록 수렴해 나가면 해결될 듯 합니다. 카메라 위치를 잘 정해, 로봇 암 적절한 곳에 마운팅해야 할 듯 하네요.

      삭제
    2. 카메라가 천장에달려서 위에서 아래의 사물을 2d 평면상해서 인식해서 로봇팔이 사물을 잡앗으면 하는데요.. 그 물체의 위치인식에따른 로봇팔의 아두이노소스를 어떻게짜야할까요..? 오픈씨브이와의 연계를 어떻게해야할지 감이안잡힙니다.. 도와주세요 ! ㅠ

      삭제
    3. OpenCV 예제에 보시면, 2D 이미지에서 세그먼트를 분리해, 위치와 크기를 인식하는 예제에 있습니다. 로봇암의 위치는 XY 위치를 역산해 각 관절의 각운동으로 변환해 주시면 될 듯 하네요. 역기구학을 활용하시면 될것 같습니다(http://daddynkidsmakers.blogspot.kr/2015/11/ros-moveit.html). 물체를 잡으려면 로봇암의 말단에 카메라가 달려있으면 좋겠지만, 천장에 달려 있다면, 여러가지 방법을 고민해야 할 것입니다. 천장에서 로봇암 엔드 위치를 인식할 수 있게 타겟을 다는 방법도 그 중에 하나입니다. 타겟은 구나 체커보드 타겟 등을 사용할 수 있겠죠.
      Target tracking robot 키워드로 구글링해보시면, rasberrypi 보드를 이용한 논문이나 예제 등을 찾으실수 있을 겁니다.

      삭제
  4. 안녕하세요 게시글 너무 잘 보고갑니다. 저희가 로봇암제어를 하고있는데
    PID제어를 추가해서 시리얼모니터를 통해 읽으면서 제어하고 싶은데 관련코드나 참고할만한곳 있을까요??

    답글삭제
    답글
    1. 구글링해 보시면 encoder 등 이용한 방법들과 소스가 나올겁니다(arduino motor PID code 등). 아래는 관련 동영상과 코드 링크입니다.

      https://www.youtube.com/watch?v=BIq-sIvN080
      http://blog.solutions-cubed.com/pid-motor-control-with-an-arduino/

      삭제
  5. i require a video how the interfacing is done with arduino

    답글삭제
    답글
    1. Refer to the below links.
      https://www.youtube.com/watch?v=uBP4FCDORtQ
      https://www.youtube.com/watch?v=2JImBb9YLdg

      삭제
  6. first video showing the connections. I require the how the connections made in the breadboard and arduino.
    Please make a small tutorial on connections and which pins we have to connect it effectively.

    답글삭제
    답글
    1. I useed 6 servo motors such as base, shoulder, foreArm, wristFlex, wristRotate, claw.

      The pin numbers of Arduino mega are like below.

      base = 3
      shoulder = 5
      foreArm = 6
      wristFlex = 9
      wristRotate = 10
      claw = 11

      삭제
  7. 작성자가 댓글을 삭제했습니다.

    답글삭제
  8. I am making a arm robot. it is similar to your robot but I have some problems about " external power supply for multiple servo". So can you describe detail about this? How many battery?... I saw that, you used 3 batteries, it was serial circuit (1.5*3=4.5). Is that right? In datasheet of servo, 4.8-6v is better...please help me.( My English is not good, I hope you can understand what i mean)

    답글삭제
    답글
    1. Hi. Did you connect power line of the servo with the battery and ground line of the servo with common GND with arduino? If you visit the below link, you can check it.

      https://www.youtube.com/watch?v=iBUU-lx-wPM

      삭제
  9. I know about that but I mean I ask you about external power supply. Can you describe obviously this? I saw some documents, some opinions said that. Example : if I use over 6V to supply for servo . The servo will die. The other used over 7V, even 9V for this so I am very confused. Please help me. Now I have some batteries 3.7 V recharge. It is very popular. Should I make power= 3.7*2=7.4V or use 3.7V ( parallel to increase current). I make a robot, include 5 servos ( 2 *mg 996r and 3 *s3003)
    please visit this below link.I saw he used external power supply= 9V
    https://www.youtube.com/results?search_query=arduino+project+arm+robot+control+by+poten

    답글삭제
  10. Sorry this is link
    https://www.youtube.com/watch?v=gPu47Z3j4UM&t=6s

    답글삭제
  11. Hi i want to know how this coding work on the serial terminal
    Which comment we have to give to the serial terminal

    답글삭제
    답글
    1. And aslo what commant are write in to serial tarminal,
      Is the robor arm is working automaticaly whan we upload the coad , or
      We give any command to arm

      삭제
    2. I'm sorry too late. if you can use serial ReadString(), you can send the command to control it.

      void setup(){
      Serial.begin(9600);
      }

      void loop(){
      while(Serial.available() > 0 ){
      String str = Serial.readString();
      if(str.indexOf("rotate") > -1){
      Serial.println("rotate");
      // TODO
      }
      else{
      Serial.println("unknown");
      }
      }
      }

      삭제
  12. Hello, sir. Do you happen to have the cad desing of the mechanical arm for 3D printing. I was given a similar assigment for my signal processing course and they allow us to use design already made. Can you provide me with a link or if you happen to have it I would deeply thank you forever.

    답글삭제
    답글
    1. Hi. I found the 3D design model which you can print.

      https://www.thingiverse.com/thing:30163

      However, the 3D printing model can be easily broken by impact. If there is a impact, it may be better to use more durable materials such as metal.

      삭제
  13. 작성자가 댓글을 삭제했습니다.

    답글삭제
  14. 2.로봇암 제어에 있는 링크를 타고 들어가서 나오는 로봇암을 제어하는 Dialog 라는 프로그램?은 어떻게 받을 수 있나요?

    답글삭제
    답글
    1. 2. Custom PCB using a Arduino Pro Mini as a 6 servo controller. Circuit Diagram available (pro_mini_6_Servo_controller.fzz) here.

      이 부분 말씀인가요?
      이거라면, Fritzing 회로 디자인 소프트웨어로, 아래 링크에서 다운로드 가능합니다.
      http://fritzing.org/download/0.7.0b/

      삭제
    2. https://bayesianadventures.wordpress.com/2013/11/27/arduino-pro-mini-controlling-robot-arm-via-qt-gui/
      작성자분께서 올리신 이 링크의 Dialog프로그램 입니다 ㅠ

      삭제
    3. 여기에 사용한 GUI 라이브러리는 Qt(https://www.qt.io/developers/)입니다.

      소스는 아래에 있습니다.
      https://github.com/vu2aeo/QtClasses

      삭제
  15. 안녕하세요. 6자유도 로봇팔 만들기 하고 있는데 외부 전원을 6V로 인가시키고 회로의 입력만 아두이노 우노를 이용하고 있는데 왜 아두이노 메가만 써야되는지 알 수 있을까요? 우노를 쓰니까 문제가 있는지 입력이 3개 이상 들어가는 순간부터 오작동을 일으키네요... 이것도 문제가 뭔지 아시나요?

    답글삭제
    답글
    1. PWM(Pulse Width Modulation) 출력핀인지 확인해 보시고, 해당 핀 제대로 servo와 attach되었는 지 도 확인해 보시길 바랍니다.

      삭제
  16. 그리고 맨 위의 1번 위의 단락에 큰 부하라고 하셨는데 큰 부하라면 어떤 뜻인가요?

    답글삭제
    답글
    1. 모터가 회전하여 물체를 들어올릴때 필요한 힘(물체의 무게)를 부하라고 생각하시면 됩니다. 로봇팔은 자체 자중 + 들어올려야 하는 물체 무게까지 고려해 모터를 선택해야 합니다. 아래 링크에 잘 설명되어 있으니 참고바랍니다.

      http://www.mfgkr.com/archives/5108

      모터 선택은 아래 링크 참고바랍니다.
      https://www.maxonmotor.co.kr/maxon/view/msp

      아래는 실제 여러가지 조건을 고려해 판매하고 있는 로봇암의 예입니다.
      https://www.mecademic.com/products/Meca500-small-robot-arm

      삭제
    2. 참고로, 간단한 테스트는 이 글에서 사용하는 서보모터나 로보티즈 다이나믹셀로도 충분하지만, 다르파 챌린지 같은 곳에서 사용하는 모터는 좀 더 신뢰성과 높은 토크 모터를 사용합니다.

      https://drive.tech/en/stream-content/robots-were-the-actual-winners-at-the-winter-olympics

      삭제
  17. Can u send me complete battery specifications plz do fast its urgent

    답글삭제
  18. 1.아두이노는 코딩 업로드 하지않고 위에 대로 핀하고 다연결 했는데 로봇팔만 자동적으로 멋대로 움직입니다.

    2.아두이노를 위에 대로 핀연결하고 실행 해봤는데 시리얼 모니터가 값을 인식 못합니다.

    답글삭제
    답글
    1. 안녕하세요. 6자유도 로봇팔을만들고 결선하는데 문제가 생긴것 같아서 댓글을 남깁니다.저도 위의 김상훈씨와 비슷한 문제를 가지고있는것같은데 혹시 아두이노 우노,브레드보드랑 서보모터의 GND,VCC를 어떻게 연결하셨었는지 보여주실수있나요? 말로 이해하기 좀 벅차네요.ㅠㅠ

      삭제
    2. 죄송합니다 댓글은단다는게 답글을달았네요. 근데 혹시 님은 문제 해결하셨나요?

      삭제
  19. 안녕하세요. 6자유도 로봇팔을만들고 결선하는데 문제가 생긴것 같아서 댓글을 남깁니다.저도 위의 김상훈씨와 비슷한 문제를 가지고있는것같은데 혹시 아두이노 우노,브레드보드랑 서보모터의 GND,VCC를 어떻게 연결하셨었는지 보여주실수있나요? 말로 이해하기 좀 벅차네요.ㅠㅠ

    답글삭제
  20. Is it possible to make 6-axis robot arm without inverse kinematics/DH parameter ?

    답글삭제
    답글
    1. It's simple method by using servo control. If you should control the position of end, IK will be needed.

      삭제
  21. Hello, please response. Have you used inverse kinematics on your project ?

    답글삭제
  22. Can i control this by ps2 controller/joystick ?

    답글삭제
    답글
    1. there are many examples such as https://exploreembedded.com/wiki/Analog_JoyStick_with_Arduino

      삭제
  23. 작성자가 댓글을 삭제했습니다.

    답글삭제
  24. 혹시 영상에 나온 건전지는 AA건전지3개를 넣어서 구동하셧나요?

    답글삭제
    답글
    1. 오래전 일이라 기억은 정확이 안나네요. 7.2V까지 구동전압이니 1.5 x 4 = 6.0 v 정도도 좋을 듯 합니다.

      삭제
  25. UNO보드로도 위와 같은 코드로 실행 가능한가요???

    답글삭제
    답글
    1. 네. 가능합니다. serial register 같은 기법으로 pwm 포트를 확장할 수 있습니다. 아래 참고 바랍니다.

      https://learn.adafruit.com/adafruit-arduino-lesson-4-eight-leds/the-74hc595-shift-register
      https://www.arduino.cc/en/tutorial/ShiftOut

      편하게 모터 드라이버를 사용해도 됩니다.
      https://mcuoneclipse.com/2013/06/17/tutorial-arduino-motorstepperservo-shield-part-3-74hct595-shift-register/

      삭제
  26. 혹시 전원 공급을 무엇으로 어떻게 하셨나요?

    답글삭제
  27. 혹시 전원 공급을 빵판에 꽂으셨나요? 아니면 vin단자에 꽂으셨나요?

    답글삭제
    답글
    1. 서보모터는 전원 VCC, GND 별도로 있어요. GND는 아두이노 회로와 공통접지해야 합니다. link 참고 바랍니다. https://forum.arduino.cc/index.php?topic=487809.0

      삭제
  28. 혹시 전원을 AA건전지 3개로만 주셨나요?

    답글삭제
  29. 안녕하세요 저는 선생님이 만드신 로봇과똑같은걸사서 해보고있는데 저위에 올려주신 소스를 그대로 붙여넣기하면 될까요? 스타트버튼은 없나요?

    답글삭제
    답글
    1. 댓글 늦었습니다.
      아두이노 ide에 업로드 버튼 있습니다. 아두이노 튜토리얼은 유튜브에서도 많으니 사용방법 먼저 익힌후 위 코드 사용하는 게 좋을 듯 합니다.

      삭제
  30. 로봇은 다만들었는데 아이에게 움직이는걸보여주고싶은데 힘이 드네요 꼭좀도와주세요

    답글삭제
  31. Hola, disculpa la molestia,
    como armaste el brazo...

    답글삭제
  32. 아두이노코드로 서보모터 한개씩 작동해보는 과정에서 생기는 간섭은 어떻게 없앨 수 있나요? 2번핀서보모터를 30도만큼 움직이게 명령했는데 갑자기 12번 서보모터가 90도 회전하는 경우는 어떤게 원인일까요? 물리적 간섭은 동력부족문제로 판단, 동력이 강한 서보모터로 교체해볼 예정인데, 집게축이 뜬금없이 90도회전하는 현상은 도저히 이해가 안되네요..

    답글삭제
    답글
    1. 신호 회로는 같은 VCC 라인에 연결되어 있으면, 연결된 모터들에 의해 상호영향 간섭받을 수 있습니다. 오실로스코프 있으시면 디버깅해 보시길 바랍니다. 산업용 모터 등은 참고로 신호를 줄 때 전원 회로와는 분리(isolate)합니다. 아래 링크 참고 바랍니다. https://forum.arduino.cc/t/isolating-arduino-from-h-bridge-motor-driver/320266/6

      삭제
  33. 안녕하세요 선생님~! 같은 로봇팔로 현재 테스트를 해보고 있는데요.
    모터는 180도씩 회전합니다.
    로봇에 전원을 주지 않았을 때는 관절이 더 돌아가는데 전원을 주어 아두이노로 각도를 입력하면 전원을 주지 않았을 때 보다 덜 돌아가더라구요. 관절 각도 리밋을 재는 방법이 있을까요? 아니면 아두이노에 입력하는 각도 입력이 잘못된 것인지.. 잘 모르겠습니다.
    아두이노에서 입력하는 각도는 펄스로 150~600을 입력하고 있습니다. (150이 0도, 600이 180도)

    답글삭제