逆運動(IK)とは指示された座標を元に関節の角度を求めるものです。
色々な計算手法がありますが、本章では余弦定理をメインに角度を計算してみたいと思います。
以下のようにX、Y、Z軸の方向を決め、ハンド部分は角度計算に不要なので取り外して下さい。
指定座標をposと称し、各アクチュエータと軸間の長さを次のように呼称することにします。
では三角形を見てみましょう。
三角形を作るのはD2、D3、posとなっています。
D2→D3、D3→posはそれぞれ軸の長さから求められるので、D2→posの長さが分かればこの三角形の各角度が算出可能となります。
D2→posの長さは三平方の定理から求められます。
三辺の長さが決まったので余弦定理を使ってD3の角度を算出してみましょう。
コサインの値が取得できたらアークコサインで角度にすることができます。
ここでの角度はラジアンなので180を掛けてπ(円周率)で割れば度(デグリー)に変換することもできます。
3次元においても角度の求め方は変わりませんので、D3の角度はそのまま使用します。
同様にD2の角度を求めますが、ここで求まるのはD2、D3、posを頂点とした三角形の内角です。D2への指令角度とは異なりますので、注意が必要です。
指令値を出すため、もう一つ三角形を作ります。D2からZ軸に沿ってposの位置まで移動した点をjとし、D2、j、posを頂点とした直角三角形を作成します。D2の指令角度はこの三角形のD2の角度から先ほど求めたD2の角度を差し引きした角度になります。
但し、D2とD3のAX-12+は逆向きに結合しているので、位置を指定する際には注意が必要です。
最後にD1の角度をアークタンジェントで求めれば角度計算は終了です。
ではプログラムしてみましょう。
#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)); }
座標から角度を出すことができるようになったので、これをモーションへ組み込んでみましょう。
(This host) = https://www.besttechnology.co.jp