四足歩行ロボット〜歩行理論〜 - Libraの気まぐれ記録

就活やら学会やらで記事書くテンションにならなかった．．．（あぁ，就活ぅ）

タイトル通りですが，四足歩行ロボットを製作中です．

ルーズリーフに計算式書くくらいならブログにした方がいいと思って．

とりあえず，動いている様子．

確実に重心位置が左前にある．
あと，遅い！

数値解析

脚ロボットということで運動学，逆運動学

座標系とパラメータは画像参照
f:id:Libra23:20180322102056j:plain

運動学

$L_{xy}=L_0 + L_1 * \cos(\theta_1) + L_2 * \cos(\theta_1 + \theta_2)$

$P_x = L_{xy} * \cos(\theta_0)$

$P_y = L_{xy} * \sin(\theta_0)$

$P_z = -L_1 * \sin(\theta_1) - L_2 * \sin(\theta_1 + \theta_2)$

逆運動学

$L_{AP}=\sqrt{(L_{xy}-L_0)^2+P_z^2}$

余弦定理より

$\cos(\angle BAP)=\cos(\alpha)=\frac{L_{AP}^2+L_1^2-L_2^2}{2 L_{AP} L_1}$

$\cos(\angle ABP)=\cos(\beta)=\frac{L_1^2+L_2^2-L_{AP}^2}{2 L_1 L_2}$

$\alpha=\arctan 2(\sqrt{1-\cos(\alpha)^2},\cos(\alpha))$

$\beta=\arctan 2(\sqrt{1-\cos(\beta)^2},\cos(\beta))$

以上より

$\theta_0=\arctan 2(P_y, P_x)$

$\theta_1=\arctan 2(-P_z, L_{xy} - L_0)-\alpha$

$\theta_2=\pi-\beta$

歩行パターン

とりあえず歩行

まずは直進について歩行パターンを周期関数にしてみる．

歩行パターンはクローク歩行，静歩行です．

f:id:Libra23:20180323091107p:plain

一周期について1/8で脚を上げて，1/8で下げて3/4で元の場所に戻す．

戻している間に他の三脚を動かす．

常に三脚でバランスを取っているので，四脚接地状態での重心移動モーションを含めると

f:id:Libra23:20180323132121p:plain

1/12で脚を上げて，1/12で下げて，1/12で四脚全部を接地して重心を前に動かす．3/4で他の脚を動かす．
周期2 $\pi$ の関数だとすると

// x_delta:歩幅　x_base:初期位置　z_delta:高さ　z_base:初期位置
void walk_base(double x, double x_delta, double x_base, double z_delta, double z_base, double P[]) {
  x = fmod(x, 2 * M_PI);
  if (0 <= x && x < M_PI / 6) { // move up
    P[X] = x_base + x_delta / (M_PI / 3) * x;
    P[Z] = z_base + z_delta / (M_PI / 6) * x;
  } else if (M_PI / 6 <= x && x < M_PI / 3) { // move down
    P[X] = x_base + x_delta / (M_PI / 3) * x;
    P[Z] = (z_base + z_delta) - z_delta / (M_PI / 6) * (x - M_PI / 6);
  } else { // shift
    P[X] = (x_base + x_delta) - x_delta / (5 * M_PI / 3) * (x - M_PI / 3);
    P[Z] = z_base;
  }
}

クロークなので
右後ー＞右前ー＞左後ー＞左前の順番に動かすので $\frac{\pi}{2}$ ずつずらせば良いので

// step_speed １秒に何歩進むか step/s
// x 時間 ms
void forward(double x) {
  walk_base(2 * M_PI * step_speed * x / 1000.0 + 3 * M_PI / 2, 80, -80, 50, -100, leg_pos[RIGHT_BACK]);
  walk_base(2 * M_PI * step_speed * x / 1000.0 + 2 * M_PI / 2, 80, 80, 50, -100, leg_pos[RIGHT_FRONT]);
  walk_base(2 * M_PI * step_speed * x / 1000.0 + 1 * M_PI / 2, 80, -80, 50, -100, leg_pos[LEFT_BACK]);
  walk_base(2 * M_PI * step_speed * x / 1000.0 + 0 * M_PI / 2, 80, 80, 50, -100, leg_pos[LEFT_FRONT]);
  walk_stabilizer();     // 安定化用関数
}

歩行はこれでOKなんだけど，多分転倒するよね w

安定な歩行

安定な歩行にするには重心が支持多角形内部にあればよいので
こんな感じに移動させればよい
f:id:Libra23:20180324064413p:plain
１．上げた脚を判定（LEG0）
２．上げた脚の反対の足(LEG3)と支持脚(LEG1 LEG2)を決定
３．不安定か判定ー＞上げている脚と重心の間に直線が入っていれば安定
４．現在のbと重心を通る直線の $b_G$ を計算して動かす

計算するのは4くらいかな
$a=\frac{P_{1y}-P_{2y}}{P_{1x}-P_{2y}}$
$b=P_{1y}-a*P_{1x}$
$b_G=P_{Gy}-a*P_{Gx}$
$b_0=P_{0y}-a*P_{0x}$
$b_0 > b > b_G\,b_0 < b < b_G$ なら安定
$(b_0-b) * (b_G-b)>0$ なら不安定
$\Delta y=b_G-b+b_{margin}$

重心Gについては本来ならZMPから求めるのが良いと思う．

void walk_stabilizer() {
  int up_leg, support_leg[2];
  double gravity_pos[2] = {0, 0};   // 重心
  double a;
  double b_margin, b, b_G, b_up;

  leg2Global();   // Convert leg cordinate to global
  if (leg_pos[LEFT_FRONT][Z] == leg_pos[LEFT_BACK][Z] && leg_pos[LEFT_BACK][Z] == leg_pos[RIGHT_FRONT][Z] && leg_pos[RIGHT_FRONT][Z] == leg_pos[RIGHT_BACK][Z]) {
    up_leg = 4;
    b = 0;
    for (int leg = 0; leg < LEG_NUM; leg++) {
      b += leg_pos_global[leg][Y];
    }
    b /= LEG_NUM;
    b_G = gravity_pos[Y];
    b_up = 0;
    b_margin = 0;
  } else {
    for (int leg = 0 ; leg < LEG_NUM; leg++) {
      if (leg_pos[leg][Z] > leg_pos[up_leg][Z]) {
        up_leg = leg;
      }
    }
    if (up_leg == LEFT_FRONT) {
      support_leg[0] = LEFT_BACK;
      support_leg[1] = RIGHT_FRONT;
      b_margin = 10;
    } else if (up_leg == LEFT_BACK) {
      support_leg[0] = LEFT_FRONT;
      support_leg[1] = RIGHT_BACK;
      b_margin = 10;
    } else if (up_leg == RIGHT_FRONT) {
      support_leg[0] = LEFT_FRONT;
      support_leg[1] = RIGHT_BACK;
      b_margin = -10;
    } else if (up_leg == RIGHT_BACK) {
      support_leg[0] = RIGHT_FRONT;
      support_leg[1] = LEFT_BACK;
      b_margin = -10;
    }
    a = (leg_pos_global[support_leg[0]][Y] - leg_pos_global[support_leg[1]][Y]) / (leg_pos_global[support_leg[0]][X] - leg_pos_global[support_leg[1]][X]);
    b = leg_pos_global[support_leg[1]][Y] - a * leg_pos_global[support_leg[1]][X];
    b_G = gravity_pos[Y] - a * gravity_pos[X];
    b_up = leg_pos_global[up_leg][Y] - a * leg_pos_global[up_leg][X];
  }
  if ((b_up - b) * (b_G - b) > 1) {  // Unstable
    for (int leg = 0 ; leg < LEG_NUM; leg++) {
      leg_pos[leg][Y] += (b_G + b_margin - b);
    }
  }
}

ここまで書いてあれなんだけど，今回は安定化の部分を使いませんでした．
いや，動いている間にマイコンボードが動くんだもの．重心動くんだもの．
圧力センサ買いますかぁ，，，