- はじめに — 「あのカップをここに置け」を角度6つに変換する
- 逆運動学が順運動学より難しい理由
- 2リンク腕の解析解 — 余弦定理で最後まで解く
- ヤコビアン — 関節速度が手先速度に移る行列
- 擬似逆行列で繰り返し解く数値解
- 特異点 — 物理的に何が起きているか
- 減衰最小二乗法 — 特異点付近で生き残る
- 冗長自由度と零空間
- 締めくくり — 逆運動学は答えを求める問題ではなく答えを選ぶ問題です
はじめに — 「あのカップをここに置け」を角度6つに変換する
順運動学編で作った計算器はこう使います。肩30度、肘45度を入れると、手先が(0.212028, 0.244889)にあると教えてくれます。角度を何でも入れれば答えがひとつ出て、計算は掛け算数回で終わります。
ところが実際にやりたいことは逆方向です。カップが(0.25, 0.15)にあるので手先をそこに置いてほしい、というもので、関節が何度になるべきかは自分で求めなければなりません。
この方向が逆運動学(IK)です。そして順運動学とは比べ物にならないほど難しいです。
難しさの種類が複数あるという点が重要です。計算が複雑なのではなく、問題そのものの性質が違います。ある座標には答えがまったくなく、ある座標には2つあり、ある腕では無限にあります。そして答えがある場所でも、答えの周辺の性質が場所ごとに違い、ある姿勢では手先を1ミリメートル動かすのに関節が0.15度だけ回ればよく、別の姿勢では同じ1ミリメートルに20.8度が必要です。
この記事はその4つの難しさをひとつずつ扱います。そして実際に使えるコードで終わります。
逆運動学が順運動学より難しい理由
順運動学は関数です。関節角度ベクトルを入れると手先位置がひとつ出ます。定義域のすべての点に対して正確にひとつの値があります。
逆運動学はその関数の逆を求める問題ですが、この関数は単射でも全射でもありません。
解がない場合。 腕の長さの合計より遠い点はどんな角度でも届きません。上腕0.20メートル、前腕0.15メートルの腕の最大到達距離は0.35メートルです。0.40メートル離れた点を要求すると方程式に実数解がありません。内側にも届かない領域があります。2つのリンクの長さの差である0.05メートルより近い点は、肘をどれだけ折っても届きません。
解が複数ある場合。 到達可能な点のほとんどには正確に2つの解があります。肘を上に曲げた姿勢と下に曲げた姿勢です。6自由度の産業用ロボットではこの分岐が3か所(肩、肘、手首)で起き、最大8つの解が出ます。
解が無限にある場合。 関節の数が作業次元より多いと、解が連続的に無限にあります。3リンク平面腕で2次元位置だけを指定すると、残る自由度ひとつ分だけ腕が自由にくねります。人の腕がそうです。手を一か所に固定したまま肘を上下に動かしてみると、すぐに確認できます。
閉じた解が存在しない場合。 一般的な6自由度腕の逆運動学を代数的に解くと16次方程式になり、閉じた形の解は特定の条件でのみ存在します。手首の3軸が1点で交わる構造(球状手首)がその条件の中でもっとも広く使われるもので、産業用ロボットの大多数がこの構造を採用する実質的な理由がここにあります。そうでない腕は数値解法で解く必要があります。
| 性質 | 順運動学 | 逆運動学 |
|---|---|---|
| 解の個数 | 常に正確に1つ | 0個、複数、無限個 |
| 閉じた解 | 常に存在 | 構造によって異なる |
| 計算方式 | 行列積数回 | 解析解または反復数値解 |
| 計算時間 | 一定 | 姿勢と初期値によって変わる |
| 失敗条件 | なし | 作業空間外、収束失敗、特異点 |
| 姿勢の選択 | 不要 | 複数の解からひとつを選ぶ必要あり |
最後の行が実務でもっとも多くの事故を生みます。解が2つあるときどちらを選ぶかが腕全体の姿勢を決め、毎周期独立に解いていて選択が変わると腕が一瞬でひっくり返ります。
2リンク腕の解析解 — 余弦定理で最後まで解く
もっとも単純な場合を完全に解いてみます。リンク2本、回転関節2つ、平面上の位置2つ。未知数2つに方程式2つです。
順運動学はこうでした。
x = L1·cos(θ1) + L2·cos(θ1 + θ2)
y = L1·sin(θ1) + L2·sin(θ1 + θ2)
核心のコツはθ1を先に消すことです。両方の式をそれぞれ2乗して足します。
x² + y² = L1² + L2² + 2·L1·L2·[cos(θ1)cos(θ1+θ2) + sin(θ1)sin(θ1+θ2)]
角括弧の中は余弦の差の公式なのでcos(θ2)になります。
x² + y² = L1² + L2² + 2·L1·L2·cos(θ2)
これは三角形に対する余弦定理です。2つのリンクと肩・手先を結ぶ線分が三角形を作り、θ2はその挟角の補角です。整理すると、
D = cos(θ2) = (x² + y² - L1² - L2²) / (2·L1·L2)
数字を入れてみます。目標を(0.212028, 0.244889)とすると、
x² + y² = 0.0449559 + 0.0599706 = 0.1049264
L1² + L2² = 0.0400000 + 0.0225000 = 0.0625000
2·L1·L2 = 2 × 0.20 × 0.15 = 0.06
D = (0.1049264 - 0.0625000) / 0.06 = 0.0424264 / 0.06 = 0.7071068
Dが0.7071068です。cos(45度)です。
ここでθ2 = arccos(D)として終わってはいけません。arccosは0度から180度しか返さないので負の解を失います。代わりにサインも一緒に求めてatan2に入れます。
sin(θ2) = ±√(1 - D²)
θ2 = atan2(±√(1 - D²), D)
符号2つがそのまま2つの解です。
√(1 - 0.7071068²) = √(1 - 0.5) = √0.5 = 0.7071068
θ2 = atan2(+0.7071068, 0.7071068) = +45度
θ2 = atan2(-0.7071068, 0.7071068) = -45度
次はθ1です。手先を向く方向から、三角形の内角の分だけ引きます。
θ1 = atan2(y, x) - atan2(L2·sin(θ2), L1 + L2·cos(θ2))
前の項は肩から手先を見た角度で、後ろの項は上腕がその視線からどれだけ開いているかです。θ2 = +45度の場合で計算すると、
atan2(0.244889, 0.212028) = 49.1136度
atan2(0.15 × 0.7071068, 0.20 + 0.15 × 0.7071068)
= atan2(0.1060660, 0.3060660) = 19.1136度
θ1 = 49.1136 - 19.1136 = 30.0000度
ちょうど30度が出ます。順運動学で出発したあの角度です。
θ2 = -45度を入れると後ろの項がatan2(-0.1060660, 0.3060660) = -19.1136度になり、
θ1 = 49.1136 - (-19.1136) = 68.2271度
2つ目の解は(68.2271度, -44.9999度)です。肘が反対に曲がった姿勢で、手先はまったく同じ場所にあります。
import numpy as np
L1, L2 = 0.20, 0.15
def ik_2link(x, y, elbow=+1):
"""2リンク平面腕の解析解です。elbow=+1がelbow-down、-1がelbow-upです。"""
D = (x * x + y * y - L1 * L1 - L2 * L2) / (2 * L1 * L2)
inner = 1.0 - D * D
if inner < 0:
raise ValueError(f"作業空間外です。D={D:.6f}, 1-D²={inner:.6f}")
theta2 = np.arctan2(elbow * np.sqrt(inner), D)
theta1 = np.arctan2(y, x) - np.arctan2(L2 * np.sin(theta2), L1 + L2 * np.cos(theta2))
return np.array([theta1, theta2])
def fk_2link(q):
return np.array([L1 * np.cos(q[0]) + L2 * np.cos(q[0] + q[1]),
L1 * np.sin(q[0]) + L2 * np.sin(q[0] + q[1])])
target = np.array([0.212028, 0.244889])
for elbow, name in ((+1, "elbow-down"), (-1, "elbow-up ")):
q = ik_2link(*target, elbow=elbow)
print(f"{name} θ1={np.degrees(q[0]):9.4f}度 θ2={np.degrees(q[1]):9.4f}度 FK検算={np.round(fk_2link(q), 6)}")
try:
ik_2link(0.40, 0.0)
except ValueError as e:
print("(0.40, 0.00) ->", e)
実行結果です。
elbow-down θ1= 30.0001度 θ2= 44.9999度 FK検算=[0.212028 0.244889]
elbow-up θ1= 68.2271度 θ2= -44.9999度 FK検算=[0.212028 0.244889]
(0.40, 0.00) -> 作業空間外です。D=1.625000, 1-D²=-1.640625
最後の行が作業空間外の座標が方程式でどう現れるかを示しています。(0.40, 0)を入れると、
D = (0.16 - 0.0625) / 0.06 = 1.625
1 - D² = 1 - 2.640625 = -1.640625
Dが1を超えます。コサインが1を超えることはないので、この目標を満たす角度が存在しないという意味で、平方根の中が負になって計算が止まります。
この検査を省略するとどうなるかが重要です。numpyでは負数の平方根は例外ではなくnanで、nanは静かに広がります。θ2がnanになり、θ1もnanになり、nanがサーボに角度を書く関数まで届きます。C系の言語でnanを整数に変換した結果は未定義であり、Arduinoでservo.write()にその値が入ると関節が予測できない位置に飛びます。逆運動学関数の最初の行は常に到達可能性の検査であるべきです。
2つの解のうちどちらを選ぶかもコードが決める必要があります。実務でよくある規則は3つです。ひとつ、ハードウェアの関節限界を外れる解を捨てます。ふたつ、残った解の中で現在の姿勢からの関節角度変化量が最小のものを選びます。みっつ、それでも残れば決まった姿勢(たとえば常にelbow-up)を固定します。2番目の規則が特に重要で、これがないと軌道の途中で解が切り替わり腕がひっくり返ります。
ヤコビアン — 関節速度が手先速度に移る行列
解析解は美しいですが、2リンク腕や球状手首のように構造が特別な腕でしか求められません。一般的な腕には別のアプローチが必要で、その出発点がヤコビアンです。
発想はこうです。順運動学p = f(q)を微分すると、
ṗ = J(q) · q̇
ここでJは手先座標を関節角度で偏微分した値を集めた行列です。成分で書くと、
J[i][j] = ∂(pのi番目の成分) / ∂(qのj番目の成分)
ヤコビアンは単に偏微分を格子状に並べたもので、これより難しい定義が別にあるわけではありません。
2リンク腕のヤコビアンを手で求めてみます。xをθ1で微分すると、
∂x/∂θ1 = -L1·sin(θ1) - L2·sin(θ1 + θ2)
∂x/∂θ2 = -L2·sin(θ1 + θ2)
∂y/∂θ1 = L1·cos(θ1) + L2·cos(θ1 + θ2)
∂y/∂θ2 = L2·cos(θ1 + θ2)
行列にまとめると、
J = [ -L1·s1 - L2·s12 -L2·s12 ]
[ L1·c1 + L2·c12 L2·c12 ]
s1はsin(θ1)、s12はsin(θ1+θ2)の略記です。
この行列の行列式を計算すると、驚くほど単純な結果が出ます。
det(J) = (-L1·s1 - L2·s12)(L2·c12) - (-L2·s12)(L1·c1 + L2·c12)
= -L1·L2·s1·c12 - L2²·s12·c12 + L1·L2·s12·c1 + L2²·s12·c12
= L1·L2·(s12·c1 - s1·c12)
= L1·L2·sin(θ2)
行列式がθ1とはまったく無関係で、θ2だけに依存します。 肩をどこへ回そうと腕の性質は肘角度だけで決まるという意味で、物理的には当然です。肩を回すことは腕全体を回転させるだけだからです。
そしてθ2 = 0のとき行列式は0です。腕が完全に伸びた姿勢です。ここでヤコビアンが特異になります。
擬似逆行列で繰り返し解く数値解
ヤコビアンがあれば、逆運動学を微分方程式に変えて解くことができます。
現在の関節角度がqで手先がf(q)にあり、目標がp_targetだとすると、残っている誤差はe = p_target - f(q)です。この誤差をなくすには手先をeだけ動かす必要があり、そのために関節をどれだけ動かせばよいかがJ·Δq = eです。
Jが正方で正則ならΔq = J⁻¹·eです。ところが関節数と作業次元が違うと正方になりません。このとき使うのがムーア・ペンローズ擬似逆行列J⁺です。
関節が作業次元より多ければ(冗長自由度があれば)J⁺ = Jᵀ(J·Jᵀ)⁻¹で、この解は‖Δq‖が最小の解です。関節が足りなければJ⁺ = (Jᵀ·J)⁻¹Jᵀで、この解は誤差を最小にする解です。どちらの場合もnumpy.linalg.pinvが自動で処理してくれます。
全体のアルゴリズムはこうなります。
1. 初期角度qを決める(通常は現在の姿勢)
2. e = p_target - f(q) を計算する
3. ‖e‖が十分小さければ終了
4. Δq = J(q)⁺ · e
5. q = q + Δq、2番へ
一行ずつ見ると、これは多変数ニュートン法です。手先誤差という非線形関数の零点を探す反復です。
この方法の利点は腕の構造をまったく問わないことです。ヤコビアンさえ計算できれば、関節がいくつでもどんな順序で付いていても同じように動作します。欠点は3つあります。収束が保証されず、初期値によって違う解へ行き、特異点付近で発散します。
最後の問題が深刻なので節を分けます。
特異点 — 物理的に何が起きているか
特異点はヤコビアンのランクが落ちる姿勢です。定義だけ見ると抽象的ですが、物理的にはとても具体的なことが起きます。
ある方向へ手先が行けなくなります。 腕を完全に伸ばして、手先を肩からさらに遠ざかる方向へ押してみてください。どの関節をどう回しても、その方向へは1ミリメートルも進めません。すでに最大限伸びきっているからです。ヤコビアンの像が2次元から1次元に減ったのがこの状況です。
特異点の種類は6自由度腕で3つに整理されます。
| 種類 | 姿勢 | 起きること |
|---|---|---|
| 肘特異点 | 腕が完全に伸びる | 半径方向へ進めない |
| 肩特異点 | 手首中心が1軸の回転軸上にある | 1軸がどこを向いても手首位置が同じになる |
| 手首特異点 | 手首の4軸と6軸が一直線 | 2つの軸が同じ回転を作り一方が余る |
3番目が実務でもっとも頻繁に出会う問題です。手首の軸2つが重なると、その2つは同じ仕事をすることになり、制御器は2つを互いに反対方向に猛烈に回し始めます。産業用ロボットでティーチング中に手首が急に一回転する事故の大部分がこれです。
特異点は二分法ではなく連続的な程度の問題です。その程度を測る標準的な尺度が吉川の操作性指数です。
w = √(det(J·Jᵀ))
正方ヤコビアンでは|det(J)|と同じです。そしてより実用的な尺度が特異値分解から出る最小特異値と条件数です。先ほど作った2リンク腕で確認してみます。
import numpy as np
L1, L2 = 0.20, 0.15
def jacobian(q):
s1, c1 = np.sin(q[0]), np.cos(q[0])
s12, c12 = np.sin(q[0] + q[1]), np.cos(q[0] + q[1])
return np.array([[-L1 * s1 - L2 * s12, -L2 * s12],
[ L1 * c1 + L2 * c12, L2 * c12]])
print(" θ2 det(J) L1·L2·sin(θ2) σ1 σ2 条件数")
for d2 in (90, 45, 10, 2, 0):
J = jacobian(np.radians([30, d2]))
sigma = np.linalg.svd(J, compute_uv=False)
cond = sigma[0] / sigma[1] if sigma[1] > 1e-12 else float("inf")
print(f"{d2:4}度 {np.linalg.det(J):10.7f} {L1*L2*np.sin(np.radians(d2)):12.7f}"
f" {sigma[0]:8.6f} {sigma[1]:8.6f} {cond:10.1f}")
実行結果です。
θ2 det(J) L1·L2·sin(θ2) σ1 σ2 条件数
90度 0.0300000 0.0300000 0.269451 0.111337 2.4
45度 0.0212132 0.0212132 0.351840 0.060292 5.8
10度 0.0052094 0.0052094 0.379341 0.013733 27.6
2度 0.0010470 0.0010470 0.380731 0.002750 138.5
0度 -0.0000000 0.0000000 0.380789 0.000000 inf
2列目と3列目がすべての行で一致します。手で導いたdet(J) = L1·L2·sin(θ2)が正しかったです。最後の行の-0.0000000は浮動小数点の符号付き0で、値は0です。
読むべきなのは右の3列です。σ1は肘角度が変わっても0.27から0.38の間でほぼそのままですが、σ2は0.111から0へ落ちます。条件数は2.4から無限大へ行きます。
小さな特異値がその方向への「ギア比」です。 σ2 = 0.00275ということは、その方向へ手先を1メートル動かすには関節が1/0.00275 = 364ラジアン回らなければならないという意味です。1ミリメートルなら0.364ラジアン、20.8度です。別の方向(σ1 = 0.3807)へ1ミリメートルは0.0026ラジアン、0.15度で済みます。同じ1ミリメートルが方向によって139倍違います。
減衰最小二乗法 — 特異点付近で生き残る
これで先ほどの反復法がなぜ発散するのかが明確になりました。Δq = J⁺·eでJ⁺は特異値の逆数を含み、特異値が0へ行くと逆数が発散します。
数字で見てみます。θ2 = 1度の姿勢で手先をx方向に1ミリメートル動かせと言うと、
擬似逆行列が与える|Δq| = 0.626520 rad = 35.8970度
1ミリメートルのために関節が35.9度回らなければなりません。制御周期が100Hzなら、これを10ミリ秒以内にやれという命令で、毎秒3590度です。一般的な趣味用サーボの無負荷速度は毎秒400度前後なので、9倍を超えています。実際にはサーボが最大速度で押し続けて軌道を完全に外れ、関節限界にぶつかるか電源が崩れてボードがリセットされます。
解決策は精度を少し諦めることです。逆行列を求めるとき対角線に小さな値を足します。
Δq = Jᵀ·(J·Jᵀ + λ²·I)⁻¹·e
これが減衰最小二乗法(DLS)で、ロボット工学では中村・花房・ワンプラーの1986年の研究として、数値解析一般ではレーベンバーグ・マルカート法として知られています。
λがしていることを特異値で見ると明確です。本来は各特異値が1/σとして反転しますが、減衰を入れるとσ/(σ² + λ²)になります。σがλよりずっと大きければ1/σとほぼ同じで、σが0へ行けばこの値も0へ行きます。小さな特異値方向の増幅だけを選んで抑え、残りには手を付けません。
import numpy as np
L1, L2 = 0.20, 0.15
def fk(q):
return np.array([L1 * np.cos(q[0]) + L2 * np.cos(q[0] + q[1]),
L1 * np.sin(q[0]) + L2 * np.sin(q[0] + q[1])])
def jacobian(q):
s1, c1 = np.sin(q[0]), np.cos(q[0])
s12, c12 = np.sin(q[0] + q[1]), np.cos(q[0] + q[1])
return np.array([[-L1 * s1 - L2 * s12, -L2 * s12],
[ L1 * c1 + L2 * c12, L2 * c12]])
J = jacobian(np.radians([30, 1])) # ほぼ伸びきった、特異点直前の姿勢
dx = np.array([0.001, 0.0]) # x方向へ1mm行きたいです
dq = np.linalg.pinv(J) @ dx
print(f"擬似逆行列 |Δq|={np.linalg.norm(dq):.6f} rad = {np.degrees(np.linalg.norm(dq)):8.4f}度"
f" 100Hz換算 {np.degrees(np.linalg.norm(dq))/0.01:8.1f}度/秒"
f" 実際の移動 {np.linalg.norm(J @ dq)*1000:.4f}mm")
for lam in (0.001, 0.005, 0.01, 0.05):
dq = J.T @ np.linalg.solve(J @ J.T + lam * lam * np.eye(2), dx)
print(f"DLS λ={lam:<6} |Δq|={np.linalg.norm(dq):.6f} rad = {np.degrees(np.linalg.norm(dq)):8.4f}度"
f" 100Hz換算 {np.degrees(np.linalg.norm(dq))/0.01:8.1f}度/秒"
f" 実際の移動 {np.linalg.norm(J @ dq)*1000:.4f}mm")
実行結果です。
擬似逆行列 |Δq|=0.626520 rad = 35.8970度 100Hz換算 3589.7度/秒 実際の移動 1.0000mm
DLS λ=0.001 |Δq|=0.409784 rad = 23.4789度 100Hz換算 2347.9度/秒 実際の移動 0.7585mm
DLS λ=0.005 |Δq|=0.044071 rad = 2.5251度 100Hz換算 252.5度/秒 実際の移動 0.5113mm
DLS λ=0.01 |Δq|=0.011702 rad = 0.6705度 100Hz換算 67.0度/秒 実際の移動 0.5077mm
DLS λ=0.05 |Δq|=0.001394 rad = 0.0799度 100Hz換算 8.0度/秒 実際の移動 0.4992mm
この表がトレードオフの条件をそのまま見せてくれます。
擬似逆行列は要求した1ミリメートルを正確に作ります。代わりに関節が35.9度動く必要があります。
λ = 0.01なら関節運動が0.67度に54倍減ります。 代わりに実際の移動は0.51ミリメートルで、要求の半分です。
半分しか行けなかったのは失敗に見えますが、そうではありません。これは1周期の話です。次の周期に残った誤差を再び見て、また半分ほど行きます。手先は目標に向かって少し遅く、しかし確実に近づきます。一方、擬似逆行列は1周期で到着しようとして関節が物理的限界を超え、結果として手先は目標どころかまったく違う場所へ行ってしまいます。
λの選び方には実用的な指針があります。固定値を使うなら作業精度と最大関節速度から逆算します。もっと良い方法は可変減衰で、特異点から遠い場所ではλ = 0、最小特異値が閾値を下回ればそこからλを大きくします。こうすると平常時の精度を失わずに特異点付近だけ保護がかかります。
冗長自由度と零空間
最後のひとつです。関節が作業次元より多いとどうなるか。
3リンク平面腕に2次元位置だけを指定するとします。ヤコビアンは2行3列で、ランクが2です。階数・退化次数の定理により、零空間の次元は3 - 2 = 1です。
零空間にある関節速度ベクトルはJ·Δq = 0を満たします。関節は動くのに手先はその場にとどまる運動です。 手を机に付けたまま肘を上下に動かすあの動作です。
これを使う方法が零空間射影です。
Δq = J⁺·e + (I - J⁺·J)·z
第1項が手先を目標へ連れて行き、第2項が手先にまったく影響を与えずにzが望む方向へ姿勢を押します。(I - J⁺·J)が零空間へ射影する行列です。
zに何を入れるかが冗長自由度の使い方を決めます。関節限界の中央から離れるほど大きくなる値を入れると腕が限界を避けて動き、障害物までの距離を入れると手先経路を保ったまま胴体をよけます。操作性指数を入れると特異点から自然に離れます。
import numpy as np
LINKS = (0.20, 0.15, 0.10)
def fk3(q):
a = np.cumsum(q)
return np.array([sum(L * np.cos(t) for L, t in zip(LINKS, a)),
sum(L * np.sin(t) for L, t in zip(LINKS, a))])
def jacobian3(q):
a = np.cumsum(q)
J = np.zeros((2, 3))
for j in range(3):
J[0, j] = -sum(LINKS[i] * np.sin(a[i]) for i in range(j, 3))
J[1, j] = sum(LINKS[i] * np.cos(a[i]) for i in range(j, 3))
return J
def solve_ik(target, q0, lam=0.05, max_iter=200, tol=1e-6):
"""減衰最小二乗法の反復です。特異点を通過しても発散しません。"""
q = np.array(q0, dtype=float)
for k in range(max_iter):
error = target - fk3(q)
if np.linalg.norm(error) < tol:
return q, k
J = jacobian3(q)
q = q + J.T @ np.linalg.solve(J @ J.T + lam * lam * np.eye(2), error)
return q, max_iter
target = np.array([0.30, 0.20])
q, iters = solve_ik(target, [0.1, 0.5, 0.0])
print(f"収束 {iters}回 θ={np.round(np.degrees(q), 4)}度 FK検算={np.round(fk3(q), 8)}")
J = jacobian3(q)
print(f"Jの形 {J.shape}、ランク {np.linalg.matrix_rank(J)}、零空間次元 {3 - np.linalg.matrix_rank(J)}")
N = np.eye(3) - np.linalg.pinv(J) @ J # 零空間射影行列
z = np.array([1.0, 1.0, 1.0]) # 適当な方向を入れてみます
dq_null = N @ z
print(f"零空間方向 Δq = {np.round(dq_null, 6)}")
print(f"この方向の手先速度 J·Δq = {np.round(J @ dq_null, 12)}")
step = 0.05 * dq_null / np.linalg.norm(dq_null)
print(f"関節を {np.round(np.degrees(step), 4)}度動かしたとき")
print(f" 手先の移動量 = {np.linalg.norm(fk3(q + step) - fk3(q)) * 1000:.6f} mm")
実行結果です。
収束 6回 θ=[-7.5005 67.2642 15.0619]度 FK検算=[0.29999971 0.19999971]
Jの形 (2, 3)、ランク 2、零空間次元 1
零空間方向 Δq = [ 0.038069 -0.231649 0.463807]
この方向の手先速度 J·Δq = [0. 0.]
関節を [ 0.2098 -1.2766 2.556 ]度動かしたとき
手先の移動量 = 0.059555 mm
3つの関節がそれぞれ0.21度、-1.28度、2.56度動いたのに、手先は0.06ミリメートルしか動きませんでした。完全に0でない理由は、零空間がその一点での接線方向であり、有限の一歩には2次の誤差が残るからです。一歩を半分に減らすとこの誤差は4分の1になります。
収束反復が6回だったことも注目に値します。初期姿勢から目標までかなり離れていたのに、6回でマイクロメートル水準まで詰まります。ニュートン系の方法の収束はこれほど速いです。もちろん特異点を通過したり目標が作業空間外だったりすると話が変わるので、実際のコードには反復回数の上限と最終誤差の検査が必ず必要です。上の関数がmax_iterに達しても黙って値を返してしまう点にお気づきでしたら、その通りです。実戦コードではその場合を呼び出し側が区別できる必要があります。
締めくくり — 逆運動学は答えを求める問題ではなく答えを選ぶ問題です
この記事を貫くのはひとつの事実です。逆運動学には答えが複数あるか、ないかです。
だから実際に行うことの大部分は方程式を解くことではなく、どんな答えを望むのかを明示することです。elbow-upかelbow-downか。関節限界を超える解は捨てるか。直前の姿勢からもっとも近い解を選ぶか。冗長自由度があれば余る自由を何に使うか。特異点付近で精度と関節速度のどちらを諦めるか。
これらの選択をコードに明示しないと、選択が消えるのではなく、浮動小数点演算の順序が代わりに選んでしまいます。 そしてその選択が周期ごとに変わることが、腕が急にひっくり返る理由です。
数値解法を使うとき最後に覚えておくべきなのは、反復が収束したことと答えが使い物になることは別の話だということです。収束した角度が関節限界内にあるか、途中の経路が自分の体を通過しないか、その姿勢の条件数が扱える範囲か、すべて別途確認する必要があります。
ここまでが「どこへ行くべきか」の答えです。その角度へ実際になめらかに動く仕事は制御ループ編に続きます。そしてこの記事に出てきた擬似逆行列、特異値分解、条件数のような道具をどの水準まで勉強すべきかは、ロボット工学に必要な数学編に順序立てて整理してあります。
현재 단락 (1/250)
[順運動学](/blog/electronics/2026-08-02-forward-kinematics-explained)編で作った計算器はこう使います。肩30度、肘45度を入れると、手先が`...