【Processing】平面二軸ロボットの順運動学

こんにちは。
　最近秋葉原でサーボモータを買いました。サーボで簡単にアームロボットを作りながら、ロボット制御工学の基礎を復習してゆきます。まずは、サーボを2つ使って平面二軸アームを作成し、ロボットアームを作ったお絵かきをしたいと思います。

　まずは基礎の基礎の順運動学を解いていきます。順運動学では各軸の角度を決めた時、エンドエフェクタがどの位置に来るかを計算するものです。平面二軸アームを用いると以下に示すようにXY平面上で（可動域の制限内で）自由に位置決めすることができます。このようにアームの各関節の角度が決まった時に、手先の位置を計算する方法を順運動学といいます。

※図の変更（2014/12/04 19:25）

　では、順運動学の計算を用いて平面二軸アームの可動域を可視化するプログラムを書いてみましょう。開発環境にはビジュアルプログラミングが簡単に行えるProcessingを用います。（今後Arduinoでアームを制御する際にも便利）以下にソースコードを示します。

import controlP5.*;
import java.util.ListIterator;

ControlP5 ui;
PGraphics pg;

float x1,y1,x2,y2,xe,ye;
float angle1 = 90;
float angle2;
float the1;
float the2;
final float l1 = 150;
final float l2 = 100;
final int r = 20;
int dir = 1;

void setup(){
  size(640,640);
  stroke(255,255,255);
  fill(255,255,255);
  ui = new ControlP5(this);
  
  ui.addSlider("angle1")
     .setPosition(10,10)
     .setSize(200,20)
     .setRange(-90,90)
     .setValue(-90);
  ui.addSlider("angle2")
     .setPosition(10,50)
     .setSize(200,20)
     .setRange(0,180)
     .setValue(90);   
  //set to origin.
  x1 = 300;
  y1 = 350;
  pg = createGraphics(width,height,JAVA2D);
  
}

void draw()
{
  //angle1++;
  //if(angle1>=90) angle1 = -90;
  //for(int i=0; i<=180; i++){
  // angle2=i; 
  background(0);
  the1 = angle1*PI/180;
  the2 = angle2*PI/180;
  ellipse(x1,y1,r,r);
  x2 = x1+l1*cos(the1);
  y2 = y1+l1*sin(the1);
  xe = x2+l2*cos(the2+the1);
  ye = y2+l2*sin(the2+the1);
  ellipse(x2,y2,r,r);
  ellipse(xe,ye,r,r);
  line(x1,y1,x2,y2);
  line(x2,y2,xe,ye);
  pg.beginDraw();
    pg.noStroke();
    pg.fill(0,100,100);
    pg.ellipse(xe,ye,r,r);
  pg.endDraw();
  //image(pg.get(0,0,width,height),0,0);
  //}
}

　左上に表示されるスライドバーは各関節の角度に対応しています。また、ソースコードのコメントを外すことで自動的に全ての可動域を描画することができます。以下は全ての可動域を可視化した時の動画です。アームの動作をDrawで描画しつつ、可動域を描画するためのTipsはPGraphicsを使用することです。pg.beginDrawからpg.endDraw()までに書かれた描画処理はDraw関数内のリフレッシュによっても消えない独立されたレイヤーに描画されるため、アームの動作とエンドエフェクタの軌跡を同時に示すことができます。

_kiitaniさん(@monitorgazer)が投稿した動画

　これで平面アームの順運動学を解き、各関節の角度によってエンドエフェクタがどう動くかをシミュレーションすることが出来ました。順運動学は理解しやすい反面、自由度の増加や三次元空間への拡張を考えると複雑になりすぎて現実的ではありません。そこで用いられるのが逆運動学です。これは、エンドエフェクタの座標を決定したとき、各関節角度はどのようになるかを解くものです。次回は平面二軸アームの逆運動学に取り組みます。