IK / FKを解いてみよう - UTAF Physical AI カリキュラム

ロボットアームの動きは、「関節から見る」と「手先から見る」の2つの見方で考えられます。モーター側のことばが関節角、作業する側のことばが手先位置です。この2つを行き来する計算が、順運動学（FK）と逆運動学（IK）です。

計算	入力	出力	答え（平面2リンク）
順運動学（FK）	関節角 $\theta_1,\theta_2$	手先位置 $(x,y)$	常に1つ
逆運動学（IK）	手先位置 $(x,y)$	関節角 $\theta_1,\theta_2$	通常0個・1個・2個

演習内容¶

順運動学：角度を位置へ翻訳する¶

下の図では、根元の角度 $\theta_1$ が変わるとアーム全体が回り、肘の角度 $\theta_2$ が変わるとリンク2がリンク1に対して曲がります。入力した角度の組に対して、緑の手先位置は必ず1か所に決まります。

平面2リンクアームの2つの関節が連続的に回転し、入力した関節角から手先位置が一意に決まる様子 — Figure 1:関節角が決まると手先位置が1つに決まる

リンク1の先端は

(l_1\cos\theta_1,\;l_1\sin\theta_1)

(1)

です。リンク2は $x$ 軸から見て $\theta_1+\theta_2$ の方向を向くため、そのベクトルを足せば手先位置になります。

\begin{aligned} 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{aligned}

(2)

式で覚える場所は、リンク2の角度が $\theta_2$ ではなく $\theta_1+\theta_2$ になる点です。

$l_1=0.30\ \mathrm{m}$ 、 $l_2=0.25\ \mathrm{m}$ 、 $\theta_1=30^\circ$ 、 $\theta_2=60^\circ$ を代入すると、リンク2は真上を向きます。

(x,y)=(0.260,\;0.400)\ \mathrm{m}

(3)

Pythonでは、角度をラジアンへ直してから同じ式を書くだけです。

import math


def fk(theta1_deg, theta2_deg, l1=0.30, l2=0.25):
    theta1 = math.radians(theta1_deg)
    theta2 = math.radians(theta2_deg)
    x = l1 * math.cos(theta1) + l2 * math.cos(theta1 + theta2)
    y = l1 * math.sin(theta1) + l2 * math.sin(theta1 + theta2)
    return x, y


print(fk(30, 60))
# (0.2598076211, 0.4)

逆運動学：位置から角度を逆算する¶

IKでは、目標点から角度を直接「ひらめく」必要はありません。幾何的には、次の2つの円の交点を探しています。

根元を中心とする半径 $l_1$ の円：肘が置ける場所
目標点を中心とする半径 $l_2$ の円：手先から肘までの距離を満たす場所

2つの円が交わる場所が肘です。交点が2つあれば、肘上げと肘下げの2姿勢で同じ目標へ届きます。

根元中心の円と目標点中心の円の2交点から肘上げと肘下げの2姿勢を構成し、肘下げ側の根元角を目標方向から三角形の補正角を引いて求める様子 — Figure 2:逆運動学は2円の交点として見られ、根元角は「目標方向 − 補正角」で読める

まず、根元から目標までの距離を $r=\sqrt{x^2+y^2}$ とします。根元・肘・手先でできる三角形に余弦定理を使うと、肘角が求まります。

c_2=\cos\theta_2 =\frac{x^2+y^2-l_1^2-l_2^2}{2l_1l_2}

(4)

s_2=\sin\theta_2=\pm\sqrt{1-c_2^2},\qquad \theta_2=\operatorname{atan2}(s_2,c_2)

(5)

± が2つの姿勢を生みます。 $\theta_2$ が決まったら、根元角は

\theta_1= \operatorname{atan2}(y,x) -\operatorname{atan2}(l_2s_2,\;l_1+l_2c_2)

(6)

です。第1項は「根元から目標を見る角度」、第2項は「三角形のぶんだけリンク1を戻す補正角」です。式は 「目標を見る角度 − 三角形の補正角」 と読むと、形を覚えやすくなります。

\underbrace{\theta_1}_{\text{リンク1の向き}} =\underbrace{\operatorname{atan2}(y,x)}_{\text{目標を見る角度}} -\underbrace{\operatorname{atan2}(l_2s_2,\;l_1+l_2c_2)}_{\text{三角形の補正角}}

(7)

届く・届かない・2通りを見分ける¶

2つの円の位置関係だけで、IKの答えの個数がわかります。

2リンクアームの作業領域について、領域内部では解が2つ、境界では1つ、領域外では0になることを示す3枚の図 — Figure 3:目標までの距離 $r$ とリンク長からIKの解の個数が決まる

手先が届く条件は

|l_1-l_2|\le r\le l_1+l_2

(8)

です。

目標の場所	円の交点	IKの解
作業領域の内側	2個	2個（肘上げ・肘下げ）
作業領域の境界	1個	1個（腕が伸び切る、または折り畳まれる）
作業領域の外側	0個	0個（届かない）

ここでいう作業領域は、関節が自由に回り、リンクの厚みや衝突を無視した理想モデルです。実機では関節制限や干渉によって狭くなります。また、 $l_1=l_2$ で目標が根元そのものの場合だけは、完全に折り畳んだまま $\theta_1$ を変えられるため、例外的に無数の解があります。

2つの解をコードで往復確認する¶

次の関数は、このページの座標系で elbow="down" と elbow="up" を切り替えます。浮動小数点の丸めで $c_2$ がわずかに $[-1,1]$ を外れることがあるため、到達判定のあとで範囲内へ丸めています。

import math


def ik(x, y, l1=0.30, l2=0.25, elbow="down"):
    c2 = (x * x + y * y - l1 * l1 - l2 * l2) / (2 * l1 * l2)
    if c2 < -1.0 - 1e-9 or c2 > 1.0 + 1e-9:
        return None

    c2 = max(-1.0, min(1.0, c2))
    sign = 1.0 if elbow == "down" else -1.0
    s2 = sign * math.sqrt(max(0.0, 1.0 - c2 * c2))
    theta2 = math.atan2(s2, c2)
    theta1 = math.atan2(y, x) - math.atan2(l2 * s2, l1 + l2 * c2)
    return math.degrees(theta1), math.degrees(theta2)


for branch in ("down", "up"):
    angles = ik(0.40, 0.20, elbow=branch)
    position = fk(*angles)
    print(branch, angles, position)

# down (-5.457..., 71.538...) (0.4, 0.2)
# up   (58.587..., -71.538...) (0.4, 0.2)

角度は違いますが、FKへ戻すとどちらも $(0.40,0.20)$ になります。これが FKとIKの往復確認 です。実装した式が正しいかを確かめるときは、必ずこの往復を行います。

3次元アームも「回る平面＋2リンク」に分ける¶

垂直型3自由度アームでは、腰角 $\theta_1$ でアームの乗る鉛直平面を選び、その平面内で肩と肘の2リンクIKを解きます。

垂直型3自由度アームを、上面図で腰の回転角を決める問題と、側面図で肩と肘の2リンク逆運動学を解く問題に分解する様子 — Figure 4:3次元の位置問題を、腰の1回転と鉛直面内の2リンク問題へ分解する

手首中心を $(x_w,y_w,z_w)$ 、肩の高さを $d_1$ とすると、上面図から

\theta_1=\operatorname{atan2}(y_w,x_w)

(9)

を求めます。次に、肩から見た平面内の目標

\rho=\sqrt{x_w^2+y_w^2},\qquad \zeta=z_w-d_1

(10)

を作ります。あとは $(x,y)$ を $(\rho,\zeta)$ 、 $(l_1,l_2)$ を肩・肘のリンク長 $(a_2,a_3)$ に置き換え、先ほどと同じ2リンクIKを使います。

\begin{aligned} c_3&=\frac{\rho^2+\zeta^2-a_2^2-a_3^2}{2a_2a_3},\\ s_3&=\pm\sqrt{1-c_3^2},\qquad \theta_3=\operatorname{atan2}(s_3,c_3),\\ \theta_2&=\operatorname{atan2}(\zeta,\rho) -\operatorname{atan2}(a_3s_3,\;a_2+a_3c_3). \end{aligned}

(11)

見た目は3次元でも、計算の中心は平面2リンクのままです。

実機へつなぐ前に確認する¶

IKが数値を返しても、その姿勢を実機で安全に使えるとは限りません。

複数解のうち、直前の関節角に最も近い枝を選びます。
すべての関節角が可動範囲内にあるか確認します。
自己衝突、周囲との衝突、腕が伸び切る特異姿勢を避けます。
目標角を一度に送らず、滑らかな軌道にして少しずつ送ります。
度とラジアンを混ぜないよう、関数の入口と出口で単位を明記します。

提出物¶

自分のREADME.mdに、次の3点を1つの表へまとめて提出します。

fk(30, 60) の結果
目標 $(0.40,0.20)$ に対する肘上げ・肘下げの2解
実機で2解のどちらを使うか決める基準を1文

チェックリスト¶

☐ FKが「関節角から手先位置」、IKが「手先位置から関節角」の計算だと説明できる
☐ リンク2の絶対角が $\theta_1+\theta_2$ になることを図と式で確認した
☐ IKの2解が、2つの円の2交点に対応することを説明できる
☐ 目標距離から、IKの解が0個・1個・2個のどれかを判定できる
☐ atan2(y, x) を使う理由と、度・ラジアンの違いを説明できる
☐ FKからIKを求め、もう一度FKへ戻すと同じ手先位置になることを確認した
☐ 実機では解の連続性、関節制限、衝突、軌道を確認する必要があると理解した

参考資料¶

『ロボット運動学入門第I部位置に関する運動学と軌道生成』第1章・第5章（提供資料）
Python math.atan2 documentation