Page Top

幾何学的に角度を求める anchor.png

逆運動(IK)とは指示された座標を元に関節の角度を求めるものです。
色々な計算手法がありますが、本章では余弦定理をメインに角度を計算してみたいと思います。
以下のようにX、Y、Z軸の方向を決め、ハンド部分は角度計算に不要なので取り外して下さい。

RobotHand3D.png

指定座標をposと称し、各アクチュエータと軸間の長さを次のように呼称することにします。

id_rename.png
Page Top

2次元での角度算出 anchor.png

では三角形を見てみましょう。
三角形を作るのはD2、D3、posとなっています。

triangle2D.png

D2→D3、D3→posはそれぞれ軸の長さから求められるので、D2→posの長さが分かればこの三角形の各角度が算出可能となります。
D2→posの長さは三平方の定理から求められます。

Inclined_D2pos.png

三辺の長さが決まったので余弦定理を使ってD3の角度を算出してみましょう。

cosineD3.png

コサインの値が取得できたらアークコサインで角度にすることができます。
ここでの角度はラジアンなので180を掛けてπ(円周率)で割れば度(デグリー)に変換することもできます。

Page Top

3次元での角度計算 anchor.png

3次元においても角度の求め方は変わりませんので、D3の角度はそのまま使用します。

cosineD2.png

同様にD2の角度を求めますが、ここで求まるのはD2、D3、posを頂点とした三角形の内角です。D2への指令角度とは異なりますので、注意が必要です。

triangle3D.png

指令値を出すため、もう一つ三角形を作ります。D2からZ軸に沿ってposの位置まで移動した点をjとし、D2、j、posを頂点とした直角三角形を作成します。D2の指令角度はこの三角形のD2の角度から先ほど求めたD2の角度を差し引きした角度になります。
但し、D2とD3のAX-12+は逆向きに結合しているので、位置を指定する際には注意が必要です。

最後にD1の角度をアークタンジェントで求めれば角度計算は終了です。

Page Top

プログラミング anchor.png

ではプログラムしてみましょう。

#include <math.h>
#include <fd.h>
double pos[3] = { 70.0, 70.0, 130.0 };  // 座標(x, y, z)
const int L[4] = { 42, 27, 83, 55 };  // 軸間の長さ

// RadianからDegreeへ変換
double Degree (double rad) {
  return (rad / (atan(1) * 4)) * 180.0;
}
// RadianからDynamixelの位置へ変換
uint16_t Position (double rad) {
  return 512 + Degree(rad) * 3.41;
}
uint16_t Position2 (double rad) {
  return 512 - Degree(rad) * 3.41;
}

void main (void) {
  double D2D3, D3POS, D2POS, D2j, jPOS, tmpcos1, tmpcos2, cosD3, radD1, radD2, radD3;
  fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP);
  DX_ChangeBaudrate (1000000);
  fd_Wait (2000);
  // 必要な辺の長さを求めます
  D2D3  = L[2];
  D3POS = L[3];
  D2POS = sqrt (pos[0] * pos[0] + pos[1] * pos[1] + (pos[2] - (L[0]+L[1])) * (pos[2] - (L[0]+L[1])));
  D2j = pos[2] - (L[0] + L[1]);
  jPOS = sqrt (pos[0] * pos[0] + pos[1] * pos[1]);
  // コサイン値を求めます
  tmpcos1 = (D2D3 * D2D3 + D2POS * D2POS - D3POS * D3POS) / (2 * D2D3 * D2POS);
  tmpcos2 = (D2j * D2j + D2POS * D2POS - jPOS * jPOS) / (2 * D2j * D2POS);
  cosD3 = (D2D3 * D2D3 + D3POS * D3POS - D2POS * D2POS) / (2 * D2D3 * D3POS);
  // 角度を出します
  radD1 = atan2 (pos[1], pos[0]);
  radD2 = acos(tmpcos2) - acos(tmpcos1);
  radD3 = acos(-1) - acos (cosD3);
  // AX-12+の出力軸の位置指定
  fd_DXSetPosition (1, Position(radD1));
  fd_DXSetPosition (2, Position(radD2));
  fd_DXSetPosition (3, Position2(radD3));
}

座標から角度を出すことができるようになったので、これをモーションへ組み込んでみましょう。

次のチャプターへ

FDIII-HC Starter Kit Guideへ戻る


Front page   Diff ReloadPrint View   Page list Search Recent changes   RSS of recent changes (RSS 1.0) RSS of recent changes (RSS 2.0) RSS of recent changes (RSS Atom)
Last-modified: 2013-08-07 (Wed) 20:15:33 (JST) (2522d)