ロボットアーム研修(まえけん編)

ロボットアーム研修(まえけん編) その9 ~垂直3自由度の逆運動学~

ロボットアーム研修(まえけん編) その9 ~垂直3自由度の逆運動学~ ロボットアーム研修(まえけん編)

こんにちは、まえけんです。
今回は垂直型3自由度の逆運動学を実装して動かしてみます。

垂直型3自由度の逆運動学の実装

関節の設定

今回使用する関節は以下の3箇所です。
それ以外の関節はトルクONにはしますが
角度指令値はホームポジションのまま変更しないこととします。
計算用の軸とホームポジションの設定はこちらの記事を参照してください。
ワールド座標兼Link0の座標系ワールド座標兼Link0の座標系
Link1の座標系Link1の座標系
Link3の座標系Link3の座標系

垂直型3自由度の順運動学

今回の軸構成における手先座標は以下の式で表現できます。
軸構成における手先座標の式この式をθの式に変形することで垂直型3自由度の逆運動学を解くことが出来ます。
関節角度θ1,2,3はLink0,1,3の関節角度と対応しています。
リンク長l1,l2はCRANE-X7のサイトの画像から設定しました。
今回はl1,l2共に0.25[m]としています。CRANE-X7

垂直型3自由度の逆運動学

式変形の詳細は教科書を参照ください。
手先座標が与えられた時の各関節角度は以下の式となります。
手先座標が与えられた時の各関節角度の式
θ1とθ3はそのまま求められますが、θ2はθ3が必要となるので必ずθ3を解いた後に計算してください。

実装時の注意点

計算した各関節の値をそのまま入れてもCRANE-X7は正常に動作しません。
計算に用いた関節の正回転方向とホームポジションが、サーボの正回転方向とホームポジションと異なるからです。
そこで以前の記事で書いたRead関数のように、関節角度をオフセットしながらサーボへ指令を出すWrite関数を用意します。

Write関数の実装イメージ

//手をまっすぐ伸ばした時に角度が0になるようにオフセット
for (int cnt = 0; cnt < JOINT_NUM; cnt++)
{
	set_joint_goal[cnt] = deg2value(((joint_direction[cnt]*rad[cnt]*180.0/M_PI)+(joint_offset[cnt])));
}
//角度を投げる
bulkwrite(ID, ARRAY_LENGTH(ID), ADDR_GOAL_POSITION, LEN_GOAL_POSITION, set_joint_goal);

set_joint_goalというのが、実際にDynamixelサーボモータに指令する0~4096の値となります。
radは計算で求めた各関節の値を代入します。
計算に必要なパラメータは以前の記事と同じですので、そちらを参照してください。
こういったオフセットの計算をする時や逆運動学を計算する時など
象限をまたぐときなどは注意が必要です。
動作範囲によってはどの象限へ動かすかでθを求める際に条件分岐が必要になります。
計算式を間違えたり、象限を跨いだ時の条件分岐が正しくないと想定していない動作をします。
最初はサーボへ指令を投げずに指令値を確認してから動作させるようにするとより安全です。

実際に動かしてみた

逆運動学を実装をしてみました。
今回は4つのポーズを繰り返させています。
これでアームをざっくりと動かせるようになりました!

ロボットアーム研修 垂直型3自由度アームの逆運動学

次回

逆運動学が解けて目標の位置へアームが移動するようにはなりましたが
ギュンギュンと動作するためとても危なっかしく見えます。
次回は軌道生成を行ってゆっくりとロボットアームを動かしてみたいと思います。

タイトルとURLをコピーしました