この記事の他にも，ロボティクスの辞書に様々なロボティクスに関する記事をまとめていますので、よろしければそちらもご覧ください！

事前に必要な知識（見ておいた方が良い記事）：順運動学【概要】

三角関数による順運動学の解法

（おさらい）
順運動学とは，あるロボットアームにおいて

「各関節の角度」から「エンドエフェクタの位置，姿勢」を求める

問題です．

（おさらいここまで）

今回は，「三角関数」を用いた順運動学の解法を紹介します．

2軸の回転関節を持ったロボットアーム

2軸の回転関節を持ったロボットアームの順運動学を解いてみましょう．

一気に「エンドエフェクタの位置 $(x_{\rm{e}}, y_{\rm{e}})$」まで求めても良いですが，ここでは

リンク1の根本（=原点）からみた時のリンク1の先端位置 $(x_{\rm{1\to 2}}, y_{\rm{1\to 2}})$
リンク2の根本からみた時のリンク2の先端位置（=エンドエフェクタの位置） $(x_{\rm{2\to e}}, y_{\rm{2\to e}})$
リンク1の根本（=原点）から見た時のエンドエフェクタの位置$(x_{\rm{e}}, y_{\rm{e}})$

の順番で求めていくことにします．

1. リンク1の根本（=原点）からみた時のリンク1の先端位置

「リンク1の根本から見た時のリンク1の先端位置$(x_{\rm{1\to2}},y_{\rm{1\to2}})$」は，

リンク1の長さを$L_1$
リンク1の関節角度を$\theta_1$

とした時

\begin{align}
x_{\rm{1\to 2}} &= L_1\cos(\theta_1)\\
y_{\rm{1\to 2}} &= L_1\sin(\theta_1)
\end{align}

となります¹．

2. リンク2の根本からみた時のリンク2の先端位置（=エンドエフェクタの位置）

次に，
「リンク2の根本から見た時のリンク2の先端位置$(x_{2\to e}, y_{2\to e})$」は，

リンク2の長さを$L_2$
各関節の角度を$\theta_1, \theta_2$

とした時

\begin{align}
x_{2\to e} &= L_2\cos(\theta_1+\theta_2)\\
y_{2\to e} &= L_2\sin(\theta_1+\theta_2)
\end{align}

となります．

ここで， $(x_{\rm{2\to e}}, y_{\rm{2\to e}})$ を求める際は，$\theta_1$も考慮する必要があることに注意してください．

3. リンク1の根本（=原点）から見た時のエンドエフェクタの位置

ここまでに求めた位置を足し合わせることで，「リンク1の根本（=原点）から見た時のエンドエフェクタの位置$(x_{\rm{e}}, y_{\rm{e}})$」を求めることができます．

\begin{align}
x_{\rm{e}} &= x_{\rm{1\to 2}} + x_{\rm{2\to e}} \\
y_{\rm{e}} &= y_{\rm{1\to 2}} + y_{\rm{2\to e}}
\end{align}

多くのロボットアームにおいて，

「$i$番目のリンクの根本」と「$i+1$番目のリンクの先端」の位置

は一致しているため，上記の式のように単純に足し合わせることでリンク1の根本（=原点）からみた時のエンドエフェクタの位置を求めることができます．もしも，一致していない場合はそれを考慮する必要があるので注意してください．

n軸の回転関節を持ったロボットアーム

$n$軸の関節があるロボットアームの場合も，これまでと同様に

\begin{align}
x_{\rm{e}} &= x_{\rm{1\to 2}} + x_{\rm{2\to 3}} + ... x_{\rm{n-1\to n}}\\
y_{\rm{e}} &= y_{\rm{1\to 2}} + y_{\rm{2\to 3}} + ... y_{\rm{n-1\to n}}
\end{align}

といった手順で，ロボットアームの根本から順番に解いていけばOKです．

Pythonで三角関数による順運動学を実装

ここまでで，三角関数を用いた順運動学の解法がわかったと思うので，ここからはPythonで実装してみましょう．

今回は，上記で示したような

平面2自由度ロボットアーム（回転関節）

の例を実装します．

下記コードの

各変数の設定（ここの値を変更すれば順運動学の結果が変わる）

の箇所を変更すれば順運動学の結果が変わります．
各変数の数値を色々変更して結果が変わることを確認してみてください．

import numpy as np
import matplotlib.pyplot as plt
from sympy.geometry import *

#  -------- 各変数の設定（ここの値を変更すれば順運動学の結果が変わる）------------
link1_length = 2.0; theta1 = 30 # link1の長さ, 関節角度
link2_length = 1.0; theta2 = 45 # link2の長さ, 関節角度
#  ---------------------------------------------------------------------
# 各角度をdegree -> radianに変換
theta1, theta2 = np.radians(theta1), np.radians(theta2)

# ============= 三角関数による順運動学（メイン部分）=========================
x1, y1 = 0, 0
# link1の根本から見た時の，link1の先端位置
x1_to_2 = link1_length*np.cos(theta1)
y1_to_2 = link1_length*np.sin(theta1)

# link2の根本から見た時の，link2の先端位置（=エンドエフェクタの位置）
x2_to_e = link2_length*np.cos(theta1+theta2)
y2_to_e = link2_length*np.sin(theta1+theta2)

# link1の根本（原点座標）から見た時の，エンドエフェクタの位置
xe = x1 + x1_to_2 + x2_to_e
ye = y1 + y1_to_2 + y2_to_e
# ======================================================================


# --------------- 以下、描画関連 --------------------------------------------------------
# リンクが画面内に収まるように設定
fig = plt.figure(figsize=(5,5))
ax = fig.add_subplot(1, 1, 1)
ax.set_xlim(-(link1_length+link2_length), link1_length+link2_length)
ax.set_ylim(-(link1_length+link2_length), link1_length+link2_length)

x2 = x1 + x1_to_2
y2 = y1 + y1_to_2
# 各linkを描画し、表示
ax.plot([x1, x2], [y1, y2], color="tomato") # link1の描画
ax.plot([x2, xe], [y2, ye], color="lightgreen") # link2の描画
plt.show()

結果例

まとめと「同時変換行列による解法」について少し

今回は，三角関数を使用した順運動学の解法について説明しました．

三角関数による解法には

図で説明できるので，直感的に理解できる

といったメリットがあります．

一方で，

ロボットアームの構成が複雑になると，数式も複雑になる

といったデメリットもあります．

今回は，2次元平面上のロボットアームを想定しましたが，実世界は「3次元空間」になるため，数式はもっと複雑になります．

このように，三角関数を用いた解法は直感的である一方，ロボットが複雑になると，数式を考えるのが難しくなってしまいます．

そこで，

同次変換行列

を用いた解法の登場です．

同次変換行列による解法は，三角関数による解法と比較すると直感的ではありませんが，機械的にエンドエフェクターの位置$(x_{\rm{e}}, y_{\rm{e}})$を求めることができるため，コンピュータで扱うときなども非常に便利です．

次回はこの「同次変換行列を使用した順運動学の解法」について解説します．

以上で，「三角関数による順運動学」の説明は終わりになります．ロボティクスに興味のある方のお役に少しでも立てたら幸いです．

参考文献

余談ですが，$\sin, \cos, \tan$はそれぞれの「頭文字の筆記体」をイメージすると，分子/分母の関係が覚えやすいです（参考:三角関数の基礎知識。sinθ cosθ tanθ の覚え方・弧度法・三角比の表まとめ）． ↩