角度から手先の位置を求める
順運動学（フォワードキネマティクス）は、ロボットアームの関節の角度から、手先の位置を求める計算です。角度を測るだけで、アームがどこに手を伸ばしているかが分かります。
ここで扱うのは、平面の上で動く、2つの関節の腕です。計算はすべてシミュレーションです。
このレッスンのゴール
メモ
2つの関節の腕について、リンクの長さと関節の角度から、手先の座標を求められる。
キーワード
| ことば | いみ |
|---|---|
| リンク | 腕の、まっすぐな1本の部分 |
| 関節 | リンクとリンクをつなぎ、角度を変える部分 |
| 関節角度 | 関節がどれだけ曲がっているかを表す数 |
| 手先 | 腕の先の、ものをつかむ点 |
| 順運動学 | 関節角度から手先の位置を求めること |
学生が入力するセルは、このプレビューでは空白です。
腕は2本のリンクと2つの関節でできている
1つ目の関節は原点にあり、角度 a1 は x 軸から測ります。1つ目のリンクの長さは l1 で、その先がひじです。
2つ目の関節はひじにあり、角度 a2 は1つ目のリンクの向きから測ります。2つ目のリンクの長さは l2 で、その先が手先です。
手先はひじに2つ目のリンクを足した位置
ひじの座標は、(l1 × cos a1, l1 × sin a1) です。2つ目のリンクの向きは、a1 と a2 を足した角度になります。
手先は、ひじからその向きに l2 だけ進んだ点です。だから、手先の x は l1 × cos a1 + l2 × cos(a1 + a2) になります。y は cos を sin に変えます。
l1 が 2、a1 が 30度のひじ
docker exec jupyter python3 -c "import numpy as np; print(np.round([2 * np.cos(np.radians(30)), 2 * np.sin(np.radians(30))], 3))"ここでは表示のみで、実行されません。無料アカウントを作成すると、実行ボタンで自分のラボのターミナルに入力できます。
例: l1 が 2、l2 が 1 の腕
| a1 | a2 | ひじ | 手先 |
|---|---|---|---|
| 0度 | 0度 | (2, 0) | (3, 0) |
| 90度 | 0度 | (0, 2) | (0, 3) |
| 0度 | 90度 | (2, 0) | (2, 1) |
| 90度 | 90度 | (0, 2) | (-1, 2) |
学生が入力するセルは、このプレビューでは空白です。
l2 が 1、a2 が 60度の手先
docker exec jupyter python3 -c "import numpy as np; print(np.round([2 * np.cos(np.radians(30)) + np.cos(np.radians(90)), 2 * np.sin(np.radians(30)) + np.sin(np.radians(90))], 3))"ここでは表示のみで、実行されません。無料アカウントを作成すると、実行ボタンで自分のラボのターミナルに入力できます。
手先の位置を Python で
import math
# リンクの長さ 3 と 2、関節の角度 45 度と -45 度の腕です。
l1 = 3
l2 = 2
a1 = math.radians(45)
a2 = math.radians(-45)
elbow_x = l1 * math.cos(a1)
elbow_y = l1 * math.sin(a1)
hand_x = elbow_x + l2 * math.cos(a1 + a2)
hand_y = elbow_y + l2 * math.sin(a1 + a2)
print("ひじ:", round(elbow_x, 3), round(elbow_y, 3))
print("手先:", round(hand_x, 3), round(hand_y, 3))
# a2 を 0 に変えて、もう一度実行します。
# 腕はまっすぐ伸びます。手先は原点から何の距離ですか。
手先が届く範囲には限りがある
腕がまっすぐ伸びたとき、手先は原点から l1 + l2 の距離にあり、これが届く最も遠い点です。
腕を折りたたむと、距離は l1 と l2 の差まで近づきます。この2つの距離の間にある点だけが、手先の届く範囲です。
よくある間違い
注意
a2 は、x 軸から測った角度だと考えがちです。実際は、1つ目のリンクから測った角度です。2つ目のリンクの向きは a1 + a2 です。a2 だけを cos に入れると、手先の位置がずれます。