おもちゃのロボットアームでフィジカルAIを網羅的に勉強したい〜その２：ロボットコントローラ編

Last updated at 2026-02-22Posted at 2026-01-20

はじめに

前回は最安価な4DoFロボットの調達と組み立て、及びPWMサーボドライバの評価までを解説しました。今回は、産業用ロボットコントローラのミニチュア版とも言える、ロボットアーム制御システムを構築していきます。

ロボットコントローラの概要

本記事で制作するロボットコントローラは、4個のサーボモータ（MG90S）を協調制御し、手先のグリッパーを目標座標へ移動させる役割を担います。産業用ロボットの用語では、手先で物を掴む位置を Tool Center Point (TCP) と呼びます。目標座標とこのTCPを一致させるのがコントローラの主要な機能です。

使用するサーボモータと構成

各サーボモータはPWMサーボモータドライバに接続されており、以下のチャネル番号で割り当てています。

Ch 1: 台座を水平回転させるサーボモータ
Ch 2: 肩と肘の回転に寄与するサーボモータ
Ch 3: 肘を回転させるサーボモータ
Ch 4: グリッパーの開閉を行うサーボモータ（手首は常に水平に保たれる構造）

本機は2つの平行四辺形を組み合わせたリンク構造を採用しています。この構造により、Ch 2とCh 3の動作を適切に計算することで、グリッパーを常に水平に保ったまま上下・前後へ移動させることが可能です。

台座を水平回転させるサーボモーター（Ch １）

肩と肘を回転させるサーボモーター（Ch 2）

肘を回転させるサーボモーター（Ch 3）

手首の回転は常に水平、グリッパーを動かす（閉じる・開く）サーボモーター（Ch ４）

ロボットコントローラの要件

今回はリアルタイム制御に優れたArduino UNO（MCU）を制御の核として使用します。ミリ秒・マイクロ秒オーダーの厳密なタイミング処理が必要なため、汎用OSであるLinuxよりもマイコンが適しています。

主な機能要件は以下の通りです：

逆運動学（IK）の実装: 目標座標から各関節角を算出します。
カッティングマットを活用した運用: 市販のカッティングマットを座標平面として利用します。
キャリブレーション機能: マットのメモリを基準に、パルス幅と座標の対応付けを行います。
EEPROMへのデータ保持: キャリブレーション値を保存し、再起動後も利用可能にします。
スムーズな軌道制御: 直線軌道かつ、UnityのSmoothStepのような加減速を伴う滑らかな移動を実現します。
外部インターフェース: シリアル通信経由でPCやラズパイからコマンド操作を可能にします。

逆運動学（IK）の実装

本記事で制作するロボットコントローラは、4個のサーボモータ（MG90S）を協調制御し、手先のグリッパー（TCP）を目標座標へ移動させる機能の実装を目的とします。

関節角とサーボ回転角の切り分けここで重要なのは、計算上の「関節の曲がり角度」をそのままサーボモータに送ればよいわけではない、という点です。

関節の曲がり角 ($\theta_1, \theta_2, \theta_3$): 幾何学的な計算（逆運動学）によって導き出される、アームの姿勢を決定する理論上の角度。
サーボモータの回転角 ($j_1, j_2, j_3$): 理論上の角度に基づき、平行リンク機構による干渉などを補正して、実際に各サーボへ指令として送る回転角。

以下より、目標とする3次元座標 $(x, y, z)$ から、各関節の回転角（$j_1, j_2, j_3$）を導き出すプロセスを解説します。

1. 座標系と幾何学的リンク構成の定義

本システムでは、3次元空間上の目標座標 $(x, y, z)$ を各関節の角度へ変換するため、以下の幾何学的パラメータを定義しています。

座標軸とベース回転（$J_1$）

平面視（Top View）において、ベースの回転角 $j_1$ とアームの到達距離 $r_{total}$ を決定します。

$j_1$ ($Base\ Angle$): $x$ 軸と $y$ 軸から $atan2(y, x)$ によって算出されるベースの旋回角。
$r_{total}$: ベース中心から目標地点（TCP）までの水平投影距離。$r_{total} = \sqrt{x^2 + y^2}$ で求められます。

リンク長と関節オフセット（側面視）

側面から見たアームの構成は、複数のオフセットを含んだ幾何学モデルとなります。

$L_1, L_2$: アームの主要なリンク長（$J_2$〜$J_3$、および $J_3$〜$J_4$）
OFF_J1_J2: ベース回転軸（$J_1$）から肩関節（$J_2$）までの水平方向のズレ。
L_OFF_J4_TCP / Z_OFF_J4_TCP: 手首関節（$J_4$）から実際の手先（TCP）までの水平・垂直方向のオフセット距離。
J1_BASE(BASE_H): 地面から肩関節（$J_2$）までの高さ。

計算のための補助変数

逆運動学（IK）を解くために、図中の青い破線で示された三角形の要素を導出します。

$r_{J4}$ / $z_{J4}$: 肩関節 $J_2$ から手首軸 $J_4$ までの相対的な水平距離と垂直高さ。
$s$ ($Distance$): $J_2$ と $J_4$ を結ぶ直線距離。三平方の定理を用いて $s = \sqrt{r_{J4}^2 + z_{J4}^2}$ として算出します。
$\theta_2, \theta_3$: この $s$ を一辺とする三角形の内角を余弦定理で求めることで、最終的なサーボ角 $j_2, j_3$ を決定します。

2. 余弦定理を用いた角度の導出

アームを側面から見ると、関節 $J_2, J_3, J_4$ はリンク $L_1, L_2$ と、その両端を結ぶ仮想的な線分 $s$ によって三角形を形成します。この三角形の各内角を求めるために、高校数学の「余弦定理」を活用します。

まず、ピタゴラスの定理により距離 $s$ を求めます。

$$s = \sqrt{r_{J4}^2 + z_{J4}^2}$$

三角形の3辺（$L_1, L_2, s$）が既知であるとき、余弦定理の変形公式 $\cos A = \frac{b^2 + c^2 - a^2}{2bc}$ を用いて、各内角を算出します。

肘の内角 ($\theta_3$):
$$\theta_3 = \arccos\left(\frac{L_1^2 + L_2^2 - s^2}{2 L_1 L_2}\right)$$
肩の内角 ($\theta_2$):
$$\theta_2 = \arccos\left(\frac{L_1^2 + s^2 - L_2^2}{2 L_1 s}\right)$$

3. サーボ制御角への変換と平行リンク補正

算出された幾何学的な内角を、実際のサーボ指令角へ変換します。

肩のサーボ角 ($j_2$):水平線に対する $s$ の傾きに内角 $\theta_2$ を加えます。$$j_2 = \operatorname{atan2}(z_{J4}, r_{J4}) + \theta_2$$
肘のサーボ角 ($j_3$):本機は肘のサーボがベース側に固定された平行リンク構造であるため、肩の動きが肘に干渉します。幾何学的関係（$\theta_3 = j_3 - j_2$）より、以下の補正を行います。$$j_3 = \theta_3 + j_2$$

Arduinoスケッチの生成

Geminiへこれまでの要件を与え、ロボットコントーラのスケッチを生成させました。以下、IK計算のコード抜粋です。

// 逆運動学（IK）計算
bool calculateIK(float x, float y, float z, float &j1, float &j2, float &j3) {
  // 1. ベース角度 j1 の算出
  j1 = atan2(y, x) * 180.0 / PI;

  // 2. 水平平面距離 r_total の算出（ベース中心からTCPまでの距離）
  float r_total = sqrt(x*x + y*y);

  // 3. 肩関節(J2)から手首軸(J4)までの水平距離 r_j4 と垂直距離 z_j4
  // 物理オフセットを考慮
  float r_j4 = r_total - L_OFF_J4_TCP - OFF_J1_J2;
  float z_j4 = (z + Z_OFF_J4_TCP) - BASE_H;

  // 4. 三平方の定理で J2-J4 間の直線距離 s を算出
  float s_sq = r_j4*r_j4 + z_j4*z_j4;
  float s = sqrt(s_sq);
  
  // 物理的な可動範囲チェック
  if (s > (L1 + L2) || s < abs(L1 - L2)) return false;

  // 5. 余弦定理によるアーム内角の算出
  float t3 = acos((L1*L1 + L2*L2 - s_sq) / (2.0 * L1 * L2)) * 180.0 / PI;
  float t2 = acos((L1*L1 + s_sq - L2*L2) / (2.0 * L1 * s)) * 180.0 / PI;
  
  // 6. 各サーボの角度（地面基準等）へ変換
  j2 = atan2(z_j4, r_j4) * 180.0 / PI + t2;
  j3 = t3 + j2; // 平行リンクによる補正を含む角度
  
  return true;
}

Arduinoスケッチと制御の仕組み

Geminiによって生成されたスケッチは、単にサーボを動かすだけでなく、産業用ロボットの制御論理をマイコン内に凝縮した構成になっています。

ロボットコントローラのスケッチ

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <math.h>
#include <EEPROM.h>

/**
 * 物理パラメータ (単位: mm)
 */
const float L1 = 80.0;           // リンク1の長さ（肩〜肘）
const float L2 = 80.0;           // リンク2の長さ（肘〜手首）
const float L_OFF_J4_TCP = 64.0; // 第4関節から手先(TCP)までの水平オフセット
const float Z_OFF_J4_TCP = 8.0;  // 第4関節から手先(TCP)までの垂直オフセット
const float OFF_J1_J2 = 12.0;    // ベース回転軸から第2関節までの水平オフセット
const float BASE_H = 60.0;       // 地面から第2関節（肩）までの高さ

const int STEP_DELAY = 10;       // 軌道補間のステップ間隔（ms）
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define SERVO_FREQ 50            // サーボの駆動周波数 50Hz

// 設定保存用の構造体
struct Config {
  int signature;         // EEPROM初期化チェック用
  int j_pulse[3][2];     // 各関節のキャリブレーション点（パルス幅）
  float j_angle[3][2];   // 各関節のキャリブレーション点（理論角度）
  int grip_open;         // グリッパー開時のパルス幅
  int grip_close;        // グリッパー閉時のパルス幅
  int grip_speed_ms;     // グリッパーの動作速度
  int last_p[4];         // 最後に保存されたパルス幅
} conf;

float curX = 150.0, curY = 0.0, curZ = 50.0; // 現在の論理座標
int current_us[4] = {1500, 1500, 1500, 1500}; // 現在の各サーボ出力パルス
bool moveLogEnabled = false; 

// 現在のサーボ位置をEEPROMに保存
void saveLastPos() {
  for(int i=0; i<4; i++) conf.last_p[i] = current_us[i];
  EEPROM.put(0, conf);
}

void setup() {
  Serial.begin(9600);
  pwm.begin();
  pwm.setPWMFreq(SERVO_FREQ);

  // EEPROMから設定を読み込み
  EEPROM.get(0, conf);
  // 初回起動時（署名が一致しない場合）は初期値を書き込む
  if (conf.signature != 0xABCD) {
    conf.signature = 0xABCD;
    for(int i=0; i<3; i++) {
      conf.j_pulse[i][0] = 1500; conf.j_angle[i][0] = 0.0;
      conf.j_pulse[i][1] = 2000; conf.j_angle[i][1] = 45.0; 
      conf.last_p[i] = 1500;
    }
    conf.last_p[3] = 1500;
    conf.grip_open = 1000; conf.grip_close = 2000; conf.grip_speed_ms = 300;
    EEPROM.put(0, conf);
  }
  
  // 保存されていた位置でサーボを起動（急激な移動を防ぐため）
  for(int i=0; i<4; i++) {
    current_us[i] = conf.last_p[i];
    moveServo(i, current_us[i]);
  }

  Serial.println(F("--- ROBOT SYSTEM v3.5 (Static Startup) ---"));
  Serial.print(F("Initial Pulses: "));
  for(int i=0; i<4; i++) { Serial.print(current_us[i]); Serial.print(i<3?",":"\n"); }
  Serial.println(F("System Ready (No auto-move)."));
}

// 角度からパルス幅へ変換（線形補完）
int angleToUs(int ch, float angle) {
  float p0 = (float)conf.j_pulse[ch][0], p1 = (float)conf.j_pulse[ch][1];
  float a0 = conf.j_angle[ch][0], a1 = conf.j_angle[ch][1];
  if (abs(a1 - a0) < 0.01) return (int)p0; 
  return (int)(p0 + (angle - a0) * (p1 - p0) / (a1 - a0));
}

// サーボの物理駆動
void moveServo(int ch, int us) {
  if (ch < 0 || ch > 3) return;
  current_us[ch] = us;
  // パルス幅(us)をPCA9685の12bit解像度（0-4095）に変換
  pwm.setPWM(ch, 0, (uint16_t)(us * 4096.0 / 20000.0));
}

// 逆運動学（IK）計算
bool calculateIK(float x, float y, float z, float &j1, float &j2, float &j3) {
  // 1. ベース角度 j1 の算出
  j1 = atan2(y, x) * 180.0 / PI;

  // 2. 水平平面距離 r_total の算出（ベース中心からTCPまでの距離）
  float r_total = sqrt(x*x + y*y);

  // 3. 肩関節(J2)から手首軸(J4)までの水平距離 r_j4 と垂直距離 z_j4
  // 物理オフセットを考慮
  float r_j4 = r_total - L_OFF_J4_TCP - OFF_J1_J2;
  float z_j4 = (z + Z_OFF_J4_TCP) - BASE_H;

  // 4. 三平方の定理で J2-J4 間の直線距離 s を算出
  float s_sq = r_j4*r_j4 + z_j4*z_j4;
  float s = sqrt(s_sq);
  
  // 物理的な可動範囲チェック
  if (s > (L1 + L2) || s < abs(L1 - L2)) return false;

  // 5. 余弦定理によるアーム内角の算出
  float t3 = acos((L1*L1 + L2*L2 - s_sq) / (2.0 * L1 * L2)) * 180.0 / PI;
  float t2 = acos((L1*L1 + s_sq - L2*L2) / (2.0 * L1 * s)) * 180.0 / PI;
  
  // 6. 各サーボの角度（地面基準等）へ変換
  j2 = atan2(z_j4, r_j4) * 180.0 / PI + t2;
  j3 = t3 + j2; // 平行リンクによる補正を含む角度
  
  return true;
}

// 座標指定による滑らかな移動
void moveTo(float tx, float ty, float tz, float speed) {
  float sx = curX, sy = curY, sz = curZ;
  float dist = sqrt(sq(tx - sx) + sq(ty - sy) + sq(tz - sz));
  // 移動距離とスピードから分割ステップ数を算出
  int steps = max(1, (int)((dist / max(1.0f, speed)) * 1000.0 / STEP_DELAY));
  
  for (int i = 1; i <= steps; i++) {
    float t = (float)i / steps;
    // SmoothStep（加減速）による補間
    float easedT = t * t * (3.0 - 2.0 * t);
    float j1, j2, j3;
    
    // 補間された座標でIKを解き、サーボを動かす
    if (calculateIK(sx+(tx-sx)*easedT, sy+(ty-sy)*easedT, sz+(tz-sz)*easedT, j1, j2, j3)) {
      moveServo(0, angleToUs(0, j1)); 
      moveServo(1, angleToUs(1, j2)); 
      moveServo(2, angleToUs(2, j3));
      delay(STEP_DELAY);
    }
  }
  curX = tx; curY = ty; curZ = tz;
  saveLastPos(); // 到着後に現在のパルスを保存
}

// シリアルコマンドの実行
void executeCommand(String cmd) {
  cmd.trim();
  
  // moveコマンド：x, y, z座標とスピードsを指定
  if (cmd.startsWith("move")) {
    float tx = curX, ty = curY, tz = curZ, speed = 50.0;
    if(cmd.indexOf("x=") != -1) tx = cmd.substring(cmd.indexOf("x=")+2).toFloat();
    if(cmd.indexOf("y=") != -1) ty = cmd.substring(cmd.indexOf("y=")+2).toFloat();
    if(cmd.indexOf("z=") != -1) tz = cmd.substring(cmd.indexOf("z=")+2).toFloat();
    if(cmd.indexOf("s=") != -1) speed = cmd.substring(cmd.indexOf("s=")+2).toFloat();
    moveTo(tx, ty, tz, speed);
  } 
  // calibコマンド：現在のパルス幅を指定した座標(x,y,z)の理論角度と紐付け
  else if (cmd.startsWith("calib")) {
    int ptIdx = cmd.substring(5,6).toInt(); // calib0 または calib1
    float tx=0, ty=0, tz=0;
    if(cmd.indexOf("x=") != -1) tx = cmd.substring(cmd.indexOf("x=")+2).toFloat();
    if(cmd.indexOf("y=") != -1) ty = cmd.substring(cmd.indexOf("y=")+2).toFloat();
    if(cmd.indexOf("z=") != -1) tz = cmd.substring(cmd.indexOf("z=")+2).toFloat();
    float tj1, tj2, tj3;
    if (calculateIK(tx, ty, tz, tj1, tj2, tj3)) {
      conf.j_pulse[0][ptIdx] = current_us[0]; conf.j_angle[0][ptIdx] = tj1;
      conf.j_pulse[1][ptIdx] = current_us[1]; conf.j_angle[1][ptIdx] = tj2;
      conf.j_pulse[2][ptIdx] = current_us[2]; conf.j_angle[2][ptIdx] = tj3; 
      Serial.println(F("Point registered."));
    }
  }
  // gripコマンド：open / close
  else if (cmd.startsWith("grip")) {
    int start_us = current_us[3];
    int target_us = (cmd.indexOf("open") != -1) ? conf.grip_open : conf.grip_close;
    int steps = max(1, conf.grip_speed_ms / STEP_DELAY);
    for (int i = 1; i <= steps; i++) {
      moveServo(3, start_us + (int)((target_us - start_us) * (float)i / steps));
      delay(STEP_DELAY);
    }
    saveLastPos();
  }
  // cコマンド：直接パルス幅を指定 (例: c0=1500)
  else if (cmd.startsWith("c")) { 
    int eqIdx = cmd.indexOf('=');
    if (eqIdx != -1) {
      moveServo(cmd.substring(1, eqIdx).toInt(), cmd.substring(eqIdx + 1).toInt());
      saveLastPos();
    }
  }
  // saveコマンド：設定の明示的な保存
  else if (cmd == "save") { saveLastPos(); Serial.println(F("Config Saved.")); }
  // dumpコマンド：現在のステータスを一覧表示
  else if (cmd == "dump") {
    Serial.println(F("\n--- ROBOT CONFIG & STATUS ---"));
    
    // 1. 各チャンネルのパルス/角度マッピングテーブル
    for(int i=0; i<3; i++) {
      Serial.print("Ch"); Serial.print(i);
      Serial.print(": [P0="); Serial.print(conf.j_pulse[i][0]);
      Serial.print(", A0="); Serial.print(conf.j_angle[i][0], 1);
      Serial.print("] [P1="); Serial.print(conf.j_pulse[i][1]);
      Serial.print(", A1="); Serial.print(conf.j_angle[i][1], 1);
      Serial.print("] | CUR_P="); Serial.println(current_us[i]);
    }

    Serial.println(F("----------------------------------------"));

    // 2. キャリブレーション時に登録された基準座標データ
    Serial.println(F("[Calibration Points Data]"));
    Serial.print(F(" Point 0 (calib0) Angles: "));
    Serial.print(conf.j_angle[0][0], 1); Serial.print(",");
    Serial.print(conf.j_angle[1][0], 1); Serial.print(",");
    Serial.println(conf.j_angle[2][0], 1);

    Serial.print(F(" Point 1 (calib1) Angles: "));
    Serial.print(conf.j_angle[0][1], 1); Serial.print(",");
    Serial.print(conf.j_angle[1][1], 1); Serial.print(",");
    Serial.println(conf.j_angle[2][1], 1);

    Serial.println(F("----------------------------------------"));

    // 3. 現在の論理座標とグリッパー状態
    Serial.print(F("Current Logic TCP: X=")); Serial.print(curX);
    Serial.print(F(" Y=")); Serial.print(curY);
    Serial.print(F(" Z=")); Serial.println(curZ);
    
    Serial.print(F("Gripper: Open=")); Serial.print(conf.grip_open);
    Serial.print(F(" Close=")); Serial.println(conf.grip_close);
    Serial.println(F("--- END DUMP ---\n"));
  }
}

void loop() {
  // シリアル入力の監視とコマンド解析
  if (Serial.available() > 0) {
    String input = Serial.readStringUntil('\n');
    input.trim();
    // セミコロンによる複数コマンド一括入力に対応
    int startIdx = 0;
    int delimiterIdx = input.indexOf(';');
    while (delimiterIdx != -1) {
      executeCommand(input.substring(startIdx, delimiterIdx));
      startIdx = delimiterIdx + 1;
      delimiterIdx = input.indexOf(';', startIdx);
    }
    executeCommand(input.substring(startIdx));
  }
}

スケッチの全体構造

プログラムは大きく分けて以下の3つのレイヤーで階層化されています。

1. 通信レイヤー (Serial Command Parser)：USBシリアル経由で move, calib, grip などのコマンドを受け取り、引数（座標やパルス幅）を解析します。

2. 論理レイヤー (IK & Trajectory Planner)：目標座標から逆運動学（IK）を用いて各サーボの目標角を算出します。また、現在地から目標地までを滑らかな直線軌道（SmoothStep）で補間し、ミリ秒単位のリアルタイム処理で軌道を生成します。

3. 駆動レイヤー (Servo Mapping & Driver)：算出された角度を、後述するキャリブレーション値に基づきパルス幅（$\mu\text{s}$）へ変換し、PWMドライバ経由で各サーボへ指令を送ります。

寸法計測と実施

実機の正確な寸法を計測し、ソースコードの定数に反映させます。

計測の結果、以下の値を得ました。

測定区間	寸法(mm)
BASE_H	60
OFF_J1_J2	12
L1	80
L2	80
Z_OFF_J4_TCP	64
L_OFF_J4_TCP	8

グリッパー（手先）の動作制御

本機のグリッパーは、PWMサーボドライバーCh 4に接続されたサーボモーターの回転角を変更することで開閉動作を行う仕組みになっています。

grip close / grip open というコマンドでグリッパーの開閉を操作します。内部的には、事前に定義された開閉用のパルス幅（$\mu\text{s}$）へ瞬時に切り替える単純な制御です。

ここで注目すべきは、本機が採用している 「二重平行リンク構造」 の恩恵です。肘のサーボ（Ch 3）がベース側に固定され、リンクが平行四辺形を構成しているため、肩（Ch 2）や肘がどのように動いても、手先のグリッパーは常に地面と水平な姿勢を維持します。

このメカニズムのおかげで、手首の角度計算を省略しつつ、常に「真上から物を掴む」という安定したピッキング動作が可能になっています。

キャリブレーションの仕組み：理論角とパルス幅の動的マッピング

ロボットアーム自作において、サーボホーンを「理論上の$0^\circ$」に寸分狂わず取り付けるのは不可能です。この物理的な誤差を解決するのが、2点指定による線形補完マッピングです。

各軸について、以下の2つの基準ペアをEEPROMに記録します。

Point 0: パルス幅 $P_0$ での「実測」座標から逆算された理論角 $A_0$
Point 1: パルス幅 $P_1$ での「実測」座標から逆算された理論角 $A_1$

この2点を通る直線を定義することで、任意の目標角度 $\theta$ に対する出力パルス幅 $PWM$ を以下の式で算出します。

$$PWM = P_0 + (\theta - A_0) \cdot \frac{P_1 - P_0}{A_1 - A_0}$$

この手法のメリット

組み立て誤差の吸収: サーボの取り付けが数度ズレていても、ソフト側で自動補正されます。
個体差の解消: 安価なサーボにありがちな「パルス幅に対する回転角のバラつき」を実測値ベースで正規化できます。
直感的な調整: 「パルス幅をいじってマットの目盛りに合わせ、その座標を入力する」という手順だけで、複雑な物理定数を意識せずに精度を追い込めます。

手順と実施

理論値と実機の動作を一致させるため、以下の手順でキャリブレーションを行います。

パルス指定による位置合わせ: シリアルからパルス幅（$\mu\text{s}$）を直接指定し、TCPをカッティングマット上の既知の座標へ移動させます。
データの記録: その時のパルス幅と、マットから読み取った座標をペアとして記録します。
線形補完: 2点のキャリブレーションデータに基づき、任意の角度に対するパルス幅を線形補完で算出します。
EEPROM保存: 設定値をArduinoの内蔵EEPROMに書き込みます。

キャリブレーションは、$j_2$ と $j_3$ が共に「小さい姿勢」と「大きい姿勢」の2点で行うことで、全域での精度を確保します。

$j_2$ と $j_3$ が共に「小さい姿勢」

$j_2$ と $j_3$ が共に「大きい姿勢」

キャリブレーションの実行例（シリアルモニター操作）

キャリブレーションは、Arduino IDE2のシリアルモニターから対話形式で行います。以下は、基準となる2つの姿勢（点0と点1）を登録する際の流れです。

1. 姿勢0の登録（アームを縮めた低い位置）

まず、各サーボのパルス幅（$\mu\text{s}$）を個別に調整し(c0, c1, c2)、その時のTCP座標をカッティグマットのメモリやコンベックスを使って測定します。

c0=1650
c1=1600
c2=1900
calib0 x=115 y=0 z=42

2. 姿勢1の登録（アームを伸ばした高い位置）

同様に、腕が伸びた姿勢になるよう、TCPの位置を移動させ、その時のパルス幅で登録します。

c0=2300
c1=2300
c2=2400
calib1 x=142 y=160 z=14

3. 設定の保存

登録した2点間の関係から線形補完のパラメータが計算されます。これを不揮発性メモリ（EEPROM）に保存します。

save

4. 動作確認（目標座標への自動移動）

キャリブレーション完了後は、パルス幅を意識することなく「座標」で命令できるようになります。

move x=150 y=0 z=80 s=50

全体動作の確認

ロボットコントローラが完成したので、いよいよ、コマンド指示によるアームの動作確認を行います。

ロボットコントローラへ以下のコマンドを送り、指定した場所に置かれたワークに対するピック＆プレイスを実行出来るか確認します。

grip close
move x=160 y=0 z=20
grip open
move x=160 y=140 z=60
move x=160 y=140 z=20
grip close
move x=160 y=140 z=60
move x=160 y=0 z=60
grip open

うまく動きました！

総括

2,000円台のキットでありながら、IKを実装することでグリッパーを目標位置へスムーズに移動させることが可能となりました。

開発において最も苦慮したのは、平行リンク構造に起因する内角（$\theta_2, \theta_3$）とサーボ回転角（$j_2, j_3$）の対応付けでした。この幾何学的な干渉を数式で解決したことで、正確な制御が実現しました。

モーターや油圧、エンジンといった「動力を伴うシステム」には、フィジカルAIによる効率化のチャンスが遍在しています。今回の製作を通じ、リアルタイム制御のコード生成を含め、AIとハードウェア制御の親和性を強く実感しました。

次のステップ

次回は、本コントローラを上位システムと接続します。Python（FastMCP等）を用いてロボット制御MCPサーバーを実装し、遠隔操作や高度なアプリケーションとの連携を目指します。

ソースコードの公開

以下に最新版を公開します。

You get articles that match your needs
You can efficiently read back useful information
You can use dark theme

What you can do with signing up

おもちゃのロボットアームでフィジカルAIを網羅的に勉強したい 〜 その２：ロボットコントローラ編