FDIII-HC Starter Kit Guide​/Chapter10.4 のバックアップソース(No.7) :: Besttechnology

• 差分^[1] を表示
• 現在との差分^[2] を表示
• FDIII-HC Starter Kit Guide/Chapter10.4^[3] へ行く。

*逆運動で動かす [#o66404ae]

**三角関数を使用する [#zdb959c7]
逆運動(IK)とは指示された座標を元に関節の角度を求めるものです。~
色々な計算手法がありますが、今回のロボットハンドでは三角関数をメインに角度を計算してみたいと思います。

**2次元での角度算出 [#d90f6f7a]
以降、各AX-12Aと軸の長さを次のように呼称することにします。~
#ref(id_rename.png)
では三角形を見てみましょう。~
三角形を作るのはD2、D3、posとなっています。~
#ref(triangle2D.png)
D2→D3、D3→posはそれぞれ軸の長さから求められるので、D2→posの長さが分かればこの三角形の各角度が算出可能となります。~
D2→posの長さは三平方の定理から求められます。~
#ref(inclined_D2pos.png)
三辺の長さが決まったので余弦定理を使ってD3の角度を算出してみましょう。~
#ref(cosineD3.png)
コサインの値が取得できたので、これをアークコサインで角度に直します。~
ここでの角度はラジアンなので180を掛けてπ(円周率)で割れば度(デグリー)に変換できます。~

**3次元での角度計算 [#x04dbfff]
X軸とY軸、Z軸を決めます。~
#ref(RobotHand3D.png)
3次元においても角度の求め方は変わりませんので、D3の角度はそのまま使用します。~
#ref(cosineD2.png)
同様にD2の角度を求めますが、ここで求まるのはD2、D3、posを頂点とした三角形の内角です。D2への指令角度とは異なりますので、注意が必要です。
#ref(triangle3D.png)
指令値を出すため、もう一つ三角形を作ります。D2からZ軸に沿ってposの位置まで移動した点をjとし、D2、j、posを頂点とした直角三角形を作成します。D2の指令角度はこの三角形のD2の角度から先ほど求めたD2の角度を差し引きした角度になります。

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

**条件について [#b1298fca]
本ロボットハンドの構成ではD4の角度を変更することはありません。何故なら傾きを再現するには軸が足りていないからです。~
軸を追加すると角度を求める為に回転行列や平面、ベクトル計算等が必要となり本章の主旨とは異なってしまうため、ここでは触れません。~
又、回転方向は軸を正から見て左回りになる方向がプラスとなります。

**プログラミング [#bdefef6a]
ではプログラムしてみましょう。~
#html{{
<pre class="brush:c;toolbar:false">
#include <math.h>
#include <fd.h>
double pos[3] = { 100.0, 150.0, 250.0 };  // 座標(x, y, z)
const int L[7] = { 30, 38, 82, 44, 50 };  // 軸間の長さ

void main (void) {
  // 必要な辺の長さを求めます
  double D2D3  = L[2];
  double D3POS = L[3] + L[4];
  double D2POS = sqrt (pow(pos[0], 2) + pow(pos[1], 2) + pow(pos[2], 2));
  double D2k = sqrt (pow(pos[0],2) + pow(pos[1],2) + pow(L[0]+L[1],2));
  double kPOS = sqrt (pow(pos[3]-(L[0]+L[1]),2));
  // コサイン値を求めます
  double cosD3 = (pow(D2D3,2) + pow(D3POS,2) - pow(D2POS,2)) / (2*D2D3*D3POS);
  double cosD2 = (pow(D2D3,2) + pow(D2POS,2) - pow(D3POS,2)) / (2*D2D3*D2POS);

  // 角度を出します
  Theta1 = atan2 (pos[2], pos[1]);
  Theta2 =

</pre>
}}

• FDIII-HC Starter Kit Guide/Chapter10.4 のバックアップ一覧^[6]
• FDIII-HC Starter Kit Guide/Chapter10.4 のバックアップソース(No. All)
  • 1: 2013-07-03 (水) 20:16:26^[7] yoshida^[8]
  • 2: 2013-07-08 (月) 13:44:30^[9] yoshida^[8]
  • 3: 2013-07-25 (木) 18:00:43^[10] yoshida^[8]
  • 4: 2013-07-25 (木) 19:53:01^[11] yoshida^[8]
  • 5: 2013-07-29 (月) 13:54:26^[12] yoshida^[8]
  • 6: 2013-07-29 (月) 19:56:41^[13] yoshida^[8]
  • 7: 2013-07-31 (水) 15:36:19 yoshida^[8]
  • 8: 2013-07-31 (水) 17:29:30^[14] yoshida^[8]
  • 9: 2013-08-01 (木) 19:17:51^[15] yoshida^[8]
  • 10: 2013-08-01 (木) 20:19:36^[16] yoshida^[8]
  • 11: 2013-08-02 (金) 13:14:30^[17] yoshida^[8]
  • 12: 2013-08-02 (金) 18:38:42^[18] yoshida^[8]
  • 13: 2013-08-05 (月) 13:23:30^[19] yoshida^[8]
  • 14: 2013-08-07 (水) 18:39:00^[20] yoshida^[8]
  • 現: 2013-08-07 (水) 20:15:33^[21] yoshida^[8]