これまで楽しんできたロボットアーム自作をアドベントカレンダーへの参加を機にここいらで一旦まとめてみたいと思います。

割と自由自在に動かせるようになりましたので紹介させてください。

構成

ロボットアームをPWMサーボを3つ使用して組み立てます。

部品

Arduno UNO
サーボモータMG995

　
サーボモータDS3115

動作

マウスを動かしてPC上に書いたものを、ロボットアームでダイソーの磁石のイラストボード上にリアルタイムに再現します。

消すのも自動でロボットアームがやってくれます。

紙取り換えるのしんどいから、ダイソーの磁石のイラストボードにしました。
自動で消してくれます。#ロボットアーム #ゆめ pic.twitter.com/S3K4EmtJiQ

— HomeMadeGarbage (@H0meMadeGarbage) December 10, 2019

なんか健気で可愛いですよね。

ロボットアーム

サーボモータDS3115の2つを縦方向、サーボモータMG995を首振り方向に使用しました。

ダイソーのイラストボードのペン先を切ってロボットアームのペンのフタに接着しました。

逆運動学

このロボットアームはアーム先端の座標(x, y, z)を指定して、逆運動学(Inverse Kinematics)によって各サーボの角度を算出し制御します。

ここでは座標から各サーボの角度を算出する式を導きだしてみましょう。

各アームの長さは以下の通りです。
　$L_1 = 92$ mm
　$L_2 = 137$ mm
　$L_3 = 190$ mm

　
座標は以下のように平面上を(x, y)、高さをzとします。

首振りサーボの角度$θ_1$は平面座標(x, y)ですぐ導出できます。

$$θ_1 =\tan^{-1} \left(\frac{y}{x} \right)$$

2個目のサーボの角度$θ_2$は余弦定理を用いて導出します。

${L_3}^2 = {L_2}^2 + {l_d}^2 – 2{L_2}{l_d}\cos{θ_2}’$

${θ_2}’ = \cos^{-1} \left(\frac{{L_2}^2+{l_d}^2 – {L_3}^2}{2{L_2}{l_d}} \right)$

$$θ_2 = {θ_2}’ + φ = \cos^{-1} \left(\frac{{L_2}^2+{l_d}^2 – {L_3}^2}{2{L_2}{l_d}} \right) + φ$$

$$　　　　　　但し、φ = \tan^{-1} \left(\frac{z – L_1}{l} \right)$$

$$　　　　　　　l_d = \sqrt{l^2 + \left(z – L_1\right)^2}$$

$$　　　　　　　l = \sqrt{x^2 + y^2}$$

3個目のサーボの角度$θ_3$も余弦定理を用いて導出します。

${l_d}^2 = {L_2}^2 + {L_3}^2 – 2{L_2}{L_3}\cos \left(90°+ {θ_3}’\right)= {L_2}^2 + {L_3}^2 + 2{L_2}{L_3}\sin {θ_3}’$

${θ_3}’ = \sin^{-1} \left(\frac{{l_d}^2-{L_2}^2 – {L_3}^2}{2{L_2}{L_3}} \right)$

$$θ_3 = {θ_3}’ + 90° = \sin^{-1} \left(\frac{{l_d}^2-{L_2}^2 – {L_3}^2}{2{L_2}{L_3}} \right) + 90°$$

以上のとおり、所望の座標(x, y, z)とロボットアームの長さ$L_1$、$L_2$、$L_3$からサーボの角度を導出することができました。

アーム制御システム

Processingのウィンドウ上の座標をArduinoにシリアル送信してロボットアームの平面状の座標(x, y)として制御します。

マウスをクリックするとアームについたマジックペンの先がイラストボードに接地し(z = 4)、ドラッグで線や絵を描けます。マウスがクリックされていない時は、ペン先を浮かせます(z = 35)。

キーボードの”Q”を押すと、消去作業を実行します。

Processingコード

640 × 960のウィンドウを表示して、ウィンドウ上のカーソルをマウスで動かして、座標(x, y)をPCにつながったArduinoに送信します。座標の数値は1/8に圧縮して送信しています。

マウスがクリックされていないときはz = 35を、クリックされているときはz = 4を送信します。

マウスをクリックしてドラッグするとウィンドウ上に線が描けます。キーボード”Q”で消去します。

import processing.serial.*;
Serial myPort; // シリアルポート

float i;
float j;

int x = 0, y = 200, z = 20;

void setup() {
  size(640, 960);
  strokeWeight(10);
  background(200);
  myPort = new Serial(this, Serial.list()[0], 9600);
}

void draw() {
  if (mousePressed == true){
    line(mouseX, mouseY, pmouseX, pmouseY);
    z = 4;
  }else{
    z = 35;
  }
  
  x = pmouseX/8;
  y = pmouseY/8;
  
  
  println(x + ", " + y + ", " + z);
  myPort.write('P');
  myPort.write(x);
  myPort.write(y);
  myPort.write(z);
  
  //消去
  if ((keyPressed == true) && ((key == 'q'))) {
    background(200);
    myPort.write('Q');
    myPort.write('1');
    myPort.write('1');
    myPort.write('1');
    key = '0';
  }
}

import processing.serial.*;

Serial myPort; // シリアルポート

float i;

float j;

int x = 0, y = 200, z = 20;

void setup() {

size(640, 960);

strokeWeight(10);

background(200);

myPort = new Serial(this, Serial.list()[0], 9600);

}

void draw() {

if (mousePressed == true){

line(mouseX, mouseY, pmouseX, pmouseY);

z = 4;

}else{

z = 35;

}

x = pmouseX/8;

y = pmouseY/8;

println(x + ", " + y + ", " + z);

myPort.write('P');

myPort.write(x);

myPort.write(y);

myPort.write(z);

//消去

if ((keyPressed == true) && ((key == 'q'))) {

background(200);

myPort.write('Q');

myPort.write('1');

key = '0';

}

Arduinoコード

Processingからの座標データを受けて、逆運動学でアームをリアルタイムに制御します。

ik()で逆運動学で求めた式でサーボモータの角度を算出しています。

Processingのウィンドウの座標とロボットアームの平面座標にオフセットがありますので、受信後に変換処理をしています。

キーボード”Q”のクリックによる信号でイラストボードの消去動作に入ります。

#include <Servo.h>

Servo myservo1, myservo2, myservo3;

float th1=90.0,th2=90.0,th3=90.0;
float phi,ld,l;  

float L1=92.0,L2=137.0,L3=190.0;

int x,y,z;

char c;

void setup() {
  Serial.begin(9600);
  myservo1.attach(9, 500, 2420);  //MG995
  myservo2.attach(10, 820, 2140); //DS3115
  myservo3.attach(11, 820, 2140); //DS3115
  set_servo();
}

void loop() {
  if (Serial.available() >= 4) {
    c = Serial.read();
    if (c == 'P' ) {
      x = Serial.read()-60;
      y = 260 - Serial.read();
      z = Serial.read();
      ik(x, y, z);
    }else   if (c == 'Q' ) { //消去
      ik(x, y, 60);
      delay(500);
      ik(x, 276, 60);
      delay(500);
      ik(60, 276, 60);
      delay(20);
      ik(65, 265, 60);
      delay(50);
      ik(50, 265, 60);
      delay(100);
      ik(50, 265, 0);
      for(int i = 265; i >= 220; i-=2){
        ik(50, i, 1);
        delay(50);
      }
      ik(50, 240, 40);
      delay(200);
      for(int i = 240; i >= 200; i-=2){
        ik(50, i, 1);
        delay(40);
      }
      for(int i = 230; i >= 146; i-=2){
        ik(50, i, 1);
        delay(20);
      }
      ik(50, 146, 60);
      delay(200);
      ik(65, 130, 60);
      delay(200);
      for(int i = 130; i <= 268; i+=2){
        ik(65, i, 5);
        delay(20);
      }
      ik(65, 268, 60);
      delay(100);
    }
    c = '0';
  }
}

void ik(float x,float y,float z){
  th1=atan2(y,x);
  l=sqrt(x*x + y*y);
  ld=sqrt(l*l + (z-L1)*(z-L1));
  phi=atan2((z-L1),l);
  th2=phi + acos((ld*ld+L2*L2-L3*L3)/(2.0*ld*L2));
  th3=asin((ld*ld-L2*L2-L3*L3)/(2.0*L2*L3)) + M_PI /2.0;
  th1=th1*180.0/M_PI ;
  th2=th2*180.0/M_PI ;
  th3=th3*180.0/M_PI ;
  set_servo();
}

void set_servo(){
    myservo1.write(th1);
    myservo2.write(th2);
    myservo3.write(th3);
}

#include <Servo.h>

Servo myservo1, myservo2, myservo3;

float th1=90.0,th2=90.0,th3=90.0;

float phi,ld,l;

float L1=92.0,L2=137.0,L3=190.0;

int x,y,z;

char c;

void setup() {

Serial.begin(9600);

myservo1.attach(9, 500, 2420); //MG995

myservo2.attach(10, 820, 2140); //DS3115

myservo3.attach(11, 820, 2140); //DS3115

set_servo();

}

void loop() {

if (Serial.available() >= 4) {

c = Serial.read();

if (c == 'P' ) {

x = Serial.read()-60;

y = 260 - Serial.read();

z = Serial.read();

ik(x, y, z);

}else if (c == 'Q' ) { //消去

ik(x, y, 60);

delay(500);

ik(x, 276, 60);

delay(500);

ik(60, 276, 60);

delay(20);

ik(65, 265, 60);

delay(50);

ik(50, 265, 60);

delay(100);

ik(50, 265, 0);

for(int i = 265; i >= 220; i-=2){

ik(50, i, 1);

delay(50);

}

ik(50, 240, 40);

delay(200);

for(int i = 240; i >= 200; i-=2){

ik(50, i, 1);

delay(40);

}

for(int i = 230; i >= 146; i-=2){

ik(50, i, 1);

delay(20);

}

ik(50, 146, 60);

delay(200);

ik(65, 130, 60);

delay(200);

for(int i = 130; i <= 268; i+=2){

ik(65, i, 5);

delay(20);

}

ik(65, 268, 60);

delay(100);

}

c = '0';

}

void ik(float x,float y,float z){

th1=atan2(y,x);

l=sqrt(x*x + y*y);

ld=sqrt(l*l + (z-L1)*(z-L1));

phi=atan2((z-L1),l);

th2=phi + acos((ld*ld+L2*L2-L3*L3)/(2.0*ld*L2));

th3=asin((ld*ld-L2*L2-L3*L3)/(2.0*L2*L3)) + M_PI /2.0;

th1=th1*180.0/M_PI ;

th2=th2*180.0/M_PI ;

th3=th3*180.0/M_PI ;

set_servo();

}

void set_servo(){

myservo1.write(th1);

myservo2.write(th2);

myservo3.write(th3);

}

おわりに

リーズナブルなPWMサーボモータでロボットアームが作れてしまいました。
しかも制御すための式も自分で導出し、その通りに動いたときはうれしいものです。

別に何の役にも立たない pic.twitter.com/rMgZD64pVL

— HomeMadeGarbage (@H0meMadeGarbage) December 12, 2019

　
当然ですが、産業用のロボットアームの性能には遠く及ばないので引き続き学習いたします。

☟うちのはコレ。。。。

デンソーウェーブ、日立システムズ、日立キャピタル
マジリスペクト pic.twitter.com/bbG5wEv2DZ

— HomeMadeGarbage (@H0meMadeGarbage) December 13, 2019

☟このくらいハイテクノロジーでクレイジーでありたい

ハンコ押印ロボ、ハンコを押すだけでなく冊子を分解せずに電子化する機能もあります。実際の需要はこちらの方が多いかもしれません。https://t.co/kzVncJ05bd pic.twitter.com/JE1NQk2JKK

— 石井徹 (@ishiit_aroka) December 18, 2019