2: 2013-07-08 (月) 13:44:30 yoshida ソース 現: 2013-08-07 (水) 20:15:33 yoshida ソース
Line 1: Line 1:
-*逆運動で動かすには [#o66404ae]+*逆運動で動かす [#o66404ae]
-**三角関数を使用する [#zdb959c7] +**幾何学的に角度を求める [#zdb959c7] 
-逆運動(IK)とは簡単に言ってしまえば指示された座標を元に関節の角度を求めるものです。~ +逆運動(IK)とは指示された座標を元に関節の角度を求めるものです。~ 
-色々な計算手法がありますが、今回のロボットハンドでは三角関数だけで角度を計算してみたいと思います。+色々な計算手法がありますが、本章では余弦定理をメインに角度を計算してみたいと思います。~ 
 +以下のようにX、Y、Z軸の方向を決め、ハンド部分は角度計算に不要なので取り外して下さい。~ 
 +#ref(RobotHand3D.png) 
 +指定座標をposと称し、各アクチュエータと軸間の長さを次のように呼称することにします。 
 +#ref(id_rename.png)
**2次元での角度算出 [#d90f6f7a] **2次元での角度算出 [#d90f6f7a]
 +では三角形を見てみましょう。~
 +三角形を作るのはD2、D3、posとなっています。~
#ref(triangle2D.png) #ref(triangle2D.png)
-C点がID2、B点がID3の軸となり、A点がロボットハンドの先端になります。~ +D2→D3、D3→posはそれぞれ軸の長さから求められるので、D2→posの長さが分かればこの三角形の各角度が算出可能となります。~ 
-ABCを結ぶと三角形が出来上がりです。~ +D2→posの長さは三平方の定理から求められます。~ 
-BCとABは軸間の長さになるので、ACが分かれば∠A、∠B、∠Cが算出可能となります。~ +#ref(inclined_D2pos.png) 
-ACは三平方の定理から求めます。~ +三辺の長さが決まったので余弦定理を使ってD3の角度を算出してみましょう。~ 
-#ref(inclined_AC.png) +#ref(cosineD3.png) 
-これで三辺の長さが求まったので余弦定理で角度を算出しましょう。~ +コサインの値が取得できたらアークコサインで角度にすることができます。~ 
-#ref(cosine.png) +ここでの角度はラジアンなので180を掛けてπ(円周率)で割れば度(デグリー)に変換することもできます。~
-まずはこれを変形して∠Aのコサインを求めます。~ +
-#ref(cosineA.png) +
-コサインの値が取得できたので、これをアークコサインで角度に直します。~ +
-ここでの角度はラジアンなので180倍してπ(円周率)で割れば度°(デグリー)に変換できます。~ +
-引き続き∠Bと∠Cを求めます。~ +
-#ref(cosineBC.png)+
**3次元での角度計算 [#x04dbfff] **3次元での角度計算 [#x04dbfff]
-では実際の座標を計算してみましょう。+3次元においても角度の求め方は変わりませんので、D3の角度はそのまま使用します。~ 
 +#ref(cosineD2.png) 
 +同様にD2の角度を求めますが、ここで求まるのはD2、D3、posを頂点とした三角形の内角です。D2への指令角度とは異なりますので、注意が必要です。 
 +#ref(triangle3D.png) 
 +指令値を出すため、もう一つ三角形を作ります。D2からZ軸に沿ってposの位置まで移動した点をjとし、D2、j、posを頂点とした直角三角形を作成します。D2の指令角度はこの三角形のD2の角度から先ほど求めたD2の角度を差し引きした角度になります。~ 
 +但し、D2とD3のAX-12+は逆向きに結合しているので、位置を指定する際には注意が必要です。~ 
 + 
 +最後にD1の角度をアークタンジェントで求めれば角度計算は終了です。 
 + 
 +**プログラミング [#bdefef6a] 
 +ではプログラムしてみましょう。 
 +#html{{ 
 +<pre class="brush:c;toolbar:false"> 
 +#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)); 
 +} 
 +</pre> 
 +}} 
 + 
 +座標から角度を出すことができるようになったので、これをモーションへ組み込んでみましょう。 
 + 
 +[[次のチャプターへ>FDIIICHAPTER10.5]] 
 + 
 +[[FDIII-HC Starter Kit Guide>LEARNINGGUIDE]]へ戻る
トップ   差分 リロード印刷に適した表示   全ページ一覧 単語検索 最新ページの一覧   最新ページのRSS 1.0 最新ページのRSS 2.0 最新ページのRSS Atom