スカラロボット|独自のArduinoベースのロボットを構築する方法

ロボットの位置と向きを制御する方法は2つあり、それは順運動学または逆運動学を使用しています。

順運動学は、与えられた関節角度からエンドエフェクタの位置と方向を見つける必要がある場合に使用されます。

一方、逆運動学は、エンドエフェクタの特定の位置の関節角度を見つける必要がある場合に使用されます。ほとんどの場合、ロボットがツールを特定の場所または特定のX、Y、Z座標に配置する必要があるため、この方法はロボット工学でより理にかなっています。

逆運動学では、与えられた座標に従って関節角度を計算できます。

順運動学と逆運動学の両方に使用する方程式は、三角法の規則と式に基づいています。

スカラロボットのプログラミング–Arduinoと処理コード

記事の下部に、Arduinoと処理コードの両方があります。

処理開発環境で記述されたコードで方程式がどのように表示されるかを次に示します。

// FORWARD KINEMATICS
void forwardKinematics() {
  float theta1F = theta1 * PI / 180;   // degrees to radians
  float theta2F = theta2 * PI / 180;
  xP = round(L1 * cos(theta1F) + L2 * cos(theta1F + theta2F));
  yP = round(L1 * sin(theta1F) + L2 * sin(theta1F + theta2F));
}Code language: Arduino (arduino)

したがって、順運動学では、ロボットの2つのアーム、theta1とtheta2の設定された関節角度、およびそれらの長さL1とL2に従って、エンドエフェクタのX値とY値を計算します。

一方、逆運動学では、与えられた位置またはX座標とY座標に従って、関節角度theta2とtheta1を計算します。

/ INVERSE KINEMATICS
void inverseKinematics(float x, float y) {
  theta2 = acos((sq(x) + sq(y) - sq(L1) - sq(L2)) / (2 * L1 * L2));
  if (x < 0 & y < 0) {
    theta2 = (-1) * theta2;
  }
  
  theta1 = atan(x / y) - atan((L2 * sin(theta2)) / (L1 + L2 * cos(theta2)));
  
  theta2 = (-1) * theta2 * 180 / PI;
  theta1 = theta1 * 180 / PI;

 // Angles adjustment depending in which quadrant the final tool coordinate x,y is
  if (x >= 0 & y >= 0) {       // 1st quadrant
    theta1 = 90 - theta1;
  }
  if (x < 0 & y > 0) {       // 2nd quadrant
    theta1 = 90 - theta1;
  }
  if (x < 0 & y < 0) {       // 3d quadrant
    theta1 = 270 - theta1;
    phi = 270 - theta1 - theta2;
    phi = (-1) * phi;
  }
  if (x > 0 & y < 0) {       // 4th quadrant
    theta1 = -90 - theta1;
  }
  if (x < 0 & y == 0) {
    theta1 = 270 + theta1;
  }
  
  // Calculate "phi" angle so gripper is parallel to the X axis
  phi = 90 + theta1 + theta2;
  phi = (-1) * phi;

  // Angle adjustment depending in which quadrant the final tool coordinate x,y is
  if (x < 0 & y < 0) {       // 3d quadrant
    phi = 270 - theta1 - theta2;
  }
  if (abs(phi) > 165) {
    phi = 180 + phi;
  }

  theta1=round(theta1);
  theta2=round(theta2);
  phi=round(phi);
  
  cp5.getController("j1Slider").setValue(theta1);
  cp5.getController("j2Slider").setValue(theta2);
  cp5.getController("j3Slider").setValue(phi);
  cp5.getController("zSlider").setValue(zP);
}Code language: Arduino (arduino)

位置がどの象限に設定されているかに応じて、これらの「if」ステートメントを使用して関節角度を調整します。ロボットのこの構成では、実際には2つのリンクだけで逆運動学を計算しています。私が「ファイ」と呼ぶ3番目の角度は、グリッパーの向きを設定するために使用されます。

グラフィックユーザーインターフェイスは、ProcessingIDEのcontrolP5ライブラリを使用して作成されます。このライブラリを使用すると、ボタン、スライダー、テキストフィールドなどを簡単に作成できます。

たとえば、左側のスライダーを使用して関節の角度を制御し、テキストフィールドを使用して、ロボットを移動させたい位置を入力できます。プログラムでここで実行する各アクションで、シリアルポートを介してArduinoボードにデータを送信します。

if (gripperValuePrevious != gripperValue) {
    if (activeIK == false) {     // Check whether the inverseKinematics mode is active, Executre Forward kinematics only if inverseKinematics mode is off or false
      gripperAdd = round(cp5.getController("gripperValue").getValue());
      gripperValue=gripperAdd+50;
      updateData();
      println(data);
      myPort.write(data);
    }
  }Code language: Arduino (arduino)

このデータには、関節角度、グリッパー値、速度と加速度の値、および保存ボタンと実行ボタンのどちらをクリックしたかを知るためのインジケーターが含まれています。

public void updateData() {
  data = str(saveStatus)
    +","+str(runStatus)
    +","+str(round(cp5.getController("j1Slider").getValue())) 
    +","+str(round(cp5.getController("j2Slider").getValue()))
    +","+str(round(cp5.getController("j3Slider").getValue()))
    +","+str(round(cp5.getController("zSlider").getValue()))
    +","+str(gripperValue)
    +","+str(speedSlider)
    +","+str(accelerationSlider);
}Code language: Arduino (arduino)

このすべてのデータは、Arduinoで1つの長い文字列として提供されます。したがって、ここでは、最初にその文字列からデータを抽出し、それを個別の変数に入れる必要があります。

if (Serial.available()) {
    content = Serial.readString(); // Read the incomding data from Processing
    // Extract the data from the string and put into separate integer variables (data[] array)
    for (int i = 0; i < 10; i++) {
      int index = content.indexOf(","); // locate the first ","
      data[i] = atol(content.substring(0, index).c_str()); //Extract the number from start to the ","
      content = content.substring(index + 1); //Remove the number from the string
    }
    /*
     data[0] - SAVE button status
     data[1] - RUN button status
     data[2] - Joint 1 angle
     data[3] - Joint 2 angle
     data[4] - Joint 3 angle
     data[5] - Z position
     data[6] - Gripper value
     data[7] - Speed value
     data[8] - Acceleration value
    */Code language: Arduino (arduino)

これらの変数を使用して、ロボットでアクションを実行できます。たとえば、[保存]ボタンを押すと、現在の関節角度の値が別の配列に保存されます。

// If SAVE button is pressed, store the data into the appropriate arrays
    if (data[0] == 1) {
      theta1Array[positionsCounter] = data[2] * theta1AngleToSteps; //store the values in steps = angles * angleToSteps variable
      theta2Array[positionsCounter] = data[3] * theta2AngleToSteps;
      phiArray[positionsCounter] = data[4] * phiAngleToSteps;
      zArray[positionsCounter] = data[5] * zDistanceToSteps;
      gripperArray[positionsCounter] = data[6];
      positionsCounter++;
    }Code language: Arduino (arduino)

[実行]ボタンをクリックすると、保存されているステップなどが実行されます。

ステッピングモーターの制御には、AccelStepperライブラリを使用しました。これは、複数のステッパーを同時に制御するための優れたライブラリですが、このようなロボットの制御に関しては、いくつかの制限があります。複数のステッパーを制御する場合、ライブラリは加速と減速を実装できません。これは、ロボットのスムーズな操作に重要です。

stepper1.moveTo(stepper1Position);
  stepper2.moveTo(stepper2Position);
  stepper3.moveTo(stepper3Position);
  stepper4.moveTo(stepper4Position);

  while (stepper1.currentPosition() != stepper1Position || stepper2.currentPosition() != stepper2Position || stepper3.currentPosition() != stepper3Position || stepper4.currentPosition() != stepper4Position) {
    stepper1.run();
    stepper2.run();
    stepper3.run();
    stepper4.run();
  }Code language: Arduino (arduino)

I still managed to implement acceleration and deceleration with the library, but they are not as smooth as I wanted to be.

Here are full Arduino and Processing codes for this Arduino SCARA robot project:

Wrap up

So finally, once we upload the code to the Arduino, we can run the processing program, connect the power and the scara robot will start moving to its home position.

From there on, we can do whatever we want the it. We can play around manually or set it to work automatically. Of course, we can attach any kind of end-effector and make cool stuff with it. For example, we can even attach a 3D printer hot end to the robot and so make the robot a 3D printer, or attach a laser head and make it a laser cutter. I do plan try these two ideas, so make sure you subscribe to my channel so you don’t miss them in some of my future videos.

Before this video ends, I would like to give you few more notes about this project. I found the robot to be not as rigid as I expected.

I guess the problem is that almost the entire SCARA robot, the Z-axis and the arms are supported only by the first joint. The whole weight and the inertial forces generated when moving, can make quite a stress to base where the first joint is located, and as it’s just a plastic it tends to bend a little bit. Also, these belts are not backlash free so we reduce the robot rigidity with that too. However, I think the overall project is good enough so you to learn how SCARA robots work, and gives you the courage to build one for yourself.

Feel free to ask any question in the comments section below and check my Arduino Projects Collection.