「ロボットミドルウェア(2024年4月11日)」

2024年4月11日実施の「ロボットミドルウェア」の授業資料および宿題、参考リンクを掲載しています。

「ロボットミドルウェア2024年4月11日」

1. 授業スライド

2024年度東京工業大学「ロボット技術」ロボットミドルウェア (2024年4月11日) from NoriakiAndo

授業スライドPDF

2. レポート課題

回答作成にWeb検索やChatGPTの使用も認めます。使用した際は、その旨レポートに記載してください。これらを使用したことに対して減点はしません。

ただし、プログラム作成課題は、実際に動作するかを確認して、実行結果を添付してください。プログラムの実行には paiza.io の使用を推奨します、その場合は paiza.io 上で作成したプログラムのリンク（URL）もレポートに記載してください。

https://paiza.io

2.1. 課題1 (順運動学 (2自由度)) (20点)

右の2自由度アームの順運動学を求めよ。 (関節角 $(\theta_1, \theta_2)$ がわかっているときの、手先座標 $(x, y)$ を求める。ただし、$l_1, l_2$ は既知であるとする。)

2.2. 解答1

点P2 $(x, y)$ の値を $l_1, l_2, \theta_1, \theta_2$ で表す。

まず、点P1 $(x_1, y_1)$ の座標は、

\begin{eqnarray} x_1 & = & l_1 \cos\theta_1 \\
y_1 & = & l_1 \sin\theta_1 \end{eqnarray}

点P1からみた点P2の座標 $(x_2, y_2)$ は、

\begin{eqnarray} x_2 & = & l_2 \times \cos(\theta_1 + \theta_2) \\
y_2 & = & l_2 \times \sin(\theta_1 + \theta_2) \end{eqnarray}

であり、最終的に $(x, y)$ は以下の式で表される。

\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}

2.3. 課題2 (逆運動学 (2自由度)) (20点)

右のアームの逆運動学 ($(x, y) $ が与えられたとき、 $l_1, l_2 $ を用いて $(\theta_1, \theta_2) $ を求める。) を求めよ。

なお、解は2つあるので、基本的には右図実線の方の解を求めることとする。

2.4. 解答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.5. 課題3. ロボット制御に必要な以下のプログラムを示せ（40点）

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を張り付けて提出することを推奨します。

paiza.io

参考

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.6. 解答3

逆運動学は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にアクセスして試しに実行してみましょう。

paiza.io上で実行

また、python/invkinem.py (paiza.io上で表示) では matplotlib を使用して、アームのリンクの状態を以下のように表示させることができます。(paiza.io上ではmatplotlibによるグラフ表示はできません。ローカルにダウンロードして、matplotlibでグラフ表示できる環境で実行してください。)

2.7. 授業の感想（20点）

授業の感想、プログラミング、ロボットミドルウェアに対しての感想を記載してください。

※感想も課題の一つですが、よく忘れる人がいます。忘れずに書いてください。サービス問題です。

3. 問い合わせ

n-ando@aist.go.jp までメールください。