| layout |
|---|
default |
2024年4月11日実施の「ロボットミドルウェア」の授業資料および宿題、参考リンクを掲載しています。
目次
<iframe src="https://www.slideshare.net/slideshow/embed_code/key/GjLWdIYcWHO0G?startSlide=1" width="597" height="486" frameborder="0" marginwidth="0" marginheight="0" scrolling="no" style="border:1px solid #CCC; border-width:1px; margin-bottom:5px;max-width: 100%;" allowfullscreen></iframe>回答作成にWeb検索やChatGPTの使用も認めます。使用した際は、その旨レポートに記載してください。これらを使用したことに対して減点はしません。
ただし、プログラム作成課題は、実際に動作するかを確認して、実行結果を添付してください。 プログラムの実行には paiza.io の使用を推奨します、その場合は paiza.io 上で作成したプログラムのリンク（URL）もレポートに記載してください。
右の2自由度アームの順運動学を求めよ。
(関節角
点P2
まず、点P1
\begin{eqnarray}
x_1 & = & l_1 \cos\theta_1 \\
y_1 & = & l_1 \sin\theta_1
\end{eqnarray}
点P1からみた点P2の座標
\begin{eqnarray}
x_2 & = & l_2 \times \cos(\theta_1 + \theta_2) \\
y_2 & = & l_2 \times \sin(\theta_1 + \theta_2)
\end{eqnarray}
であり、最終的に
\begin{eqnarray}
x = l_1\cos\theta_1 + l_2\cos(\theta_1 + \theta_2) \\
y = l_1\sin\theta_1 + l_2\sin(\theta_1 + \theta_2)
\end{eqnarray}
右のアームの逆運動学 ($(x, y) $ が与えられたとき、 $l_1, l_2 $ を用いて $(\theta_1, \theta_2) $ を求める。) を求めよ。
なお、解は2つあるので、基本的には右図実線の方の解を求めることとする。
図中 $\theta_1, \theta_2 $ は $\alpha, \beta, \phi $ を用いて以下の式で表される。
\begin{eqnarray}
\theta_1 & = & \frac{\pi}{2} - \alpha - \phi \\
\theta_2 & = & \pi - \beta
\end{eqnarray}
余弦定理と逆関数を使って、 $\cos\alpha, \cos\beta, \tan\phi $ の値を求めると、
\begin{eqnarray}
\cos\alpha & = & \left(\frac{l_1^2 + l_d^2 - l_2^2}{2l_1l_d}\right) \\
\cos\beta & = & \left(\frac{l_1^2 + l_2^2 - l_d^2}{2l_1l_2}\right) \\
\tan\phi & = & \frac{x}{y}
\end{eqnarray}
逆関数を用いて以下のように表すことができる。
\begin{eqnarray}
\alpha & = & \arccos\left(\frac{l_1^2 + l_d^2 - l_2^2}{2l_1l_d}\right) \\
\beta & = & \arccos\left(\frac{l_1^2 + l_2^2 - l_d^2}{2l_1l_2}\right) \\
\phi & = & \arctan\frac{x}{y}
\end{eqnarray}
以上より、求める角度は、
\begin{eqnarray}
\theta_1 & = & \frac{\pi}{2} - \arccos\left(\frac{l_1^2 + l_d^2 - l_2^2}{2l_1l_d}\right) - \arctan\frac{x}{y} \\
\theta_2 & = & \pi - \arccos\left(\frac{l_1^2 + l_2^2 - l_d^2}{2l_1l_2}\right)
\end{eqnarray}
なお、この逆運動学には以下のもう一つの解があります。
\begin{eqnarray}
\theta_1 & = & \frac{\pi}{2} + \arccos\left(\frac{l_1^2 + l_d^2 - l_2^2}
{2l_1l_d}\right) - \arctan\frac{x}{y} \\
\theta_2 & = & -\pi + \arccos\left(\frac{l_1^2 + l_2^2 + l_d^2}{2l_1l_2}\right)
\end{eqnarray}
2自由度のアームの逆運動学を計算する以下の仕様の関数のPythonプログラムを作成し、次のプログラムを完成させ、実行結果を示せ。
（注）以下にプログラム例を示します。このプログラムを完成させてください。このプログラでは、path変数に代入されている手先目標座標列に対する関節角度を順に出力します。 順運動学も上で求められているので、検算結果を表示しても良いでしょう。
import math
def invkinem(link, pos):
l1 = link[0]
l2 = link[1]
x = pos[0]
y = pos[1]
ld =
b =
a =
phi =
th =
th[0] =
th[1] =
return th
link = (1.0, 1.0)
path = ((-1.0, 1.0), (-0.5, 1.0), (0.0, 1.0), (0.5, 1.0), (1.0,1.0))
for pos in path:
print(invkinem(link, pos))プログラミングには paiza.io の利用を推奨します。 paiza.ioはブラウザ上で様々なプログラミング言語を利用してプログラムの記述・実行ができるサイトです。 アカウント登録をすると、作成したプログラムを保存することもできます。 レポートに保存したプログラムのURLを張り付けて提出することを推奨します。
Pythonで数学関数を表記する方法は以下の通り。上記プログラムでは冒頭 import math として数学パッケージ math をインポートしている。mathパッケージ内には様々な数学関数が含まれており、主なものとしては以下のような関数が使える。（累乗は標準文法。）
- 累乗 x^n : x ** n (ex. x^2 → x ** 2)
- ルート √ : math.sqrt()
- 逆正接 arctan: math.atan2(x, y) = arctan(x/y)
- 逆余弦 arccos: math.acos(x)
- 円周率 : math.pi逆運動学は2つの解があり、以下の二通りのプログラムが正解となります。
逆運動学は2つの解があり、以下の二通りのプログラムを正解とします。
import math
def invkinem(link, pos):
l1 = link[0]
l2 = link[1]
x = pos[0]
y = pos[1]
ld = math.sqrt(x ** 2 + y ** 2)
b = math.acos((l1 ** 2 + l2 ** 2 - ld ** 2) / (2 * l1 * l2))
a = math.acos((l1 ** 2 + ld ** 2 - l2 ** 2) / (2 * l1 * ld))
phi = math.atan2(x, y)
th = [0] * 2
th[0] = (math.pi / 2) - a - phi
th[1] = math.pi - b
return th
link = (1.0, 1.0)
path = ((-1.0, 1.0), (-0.5, 1.0), (0.0, 1.0), (0.5, 1.0), (1.0,1.0))
for pos in path:
print invkinem(link, pos)import math
def invkinem(link, pos):
l1 = link[0]
l2 = link[1]
x = pos[0]
y = pos[1]
ld = math.sqrt(x * x + y * y)
b = math.acos((l1 * l1 + l2 * l2 - ld * ld) / (2 * l1 * l2))
a = math.acos((l1 * l1 + ld * ld - l2 * l2) / (2 * l1 * ld))
phi = math.atan2(x, y)
th = [0] * 2
th[0] = math.pi / 2 + a - phi
th[1] = - math.pi + b
return th
link = (1.0, 1.0)
path = ((-1.0, 1.0), (-0.5, 1.0), (0.0, 1.0), (0.5, 1.0), (1.0,1.0))
for pos in path:
print invkinem(link, pos)以下の paiza.io 上のプログラムでは、以上の2通りのプログラムを実行し逆運動学を求めたうえで、順運動学で検算をしています。 以下のURLにアクセスして試しに実行してみましょう。
また、python/invkinem.py (paiza.io上で表示) では matplotlib を使用して、アームのリンクの状態を以下のように表示させることができます。(paiza.io上ではmatplotlibによるグラフ表示はできません。ローカルにダウンロードして、matplotlibでグラフ表示できる環境で実行してください。)
授業の感想、プログラミング、ロボットミドルウェアに対しての感想を記載してください。
※感想も課題の一つですが、よく忘れる人がいます。忘れずに書いてください。サービス問題です。
n-ando@aist.go.jp までメールください。