ヤコビ行列と速度運動学
平面アームと空間アームの幾何ヤコビ行列を手で導出して有限差分で確認し、可操作度・特異点・分解速度制御に活用します
このレッスンで学ぶこと
- 平面2リンクアームの順運動学を微分してヤコビ行列を導出し、中心差分で検証する
- 関節軸と関節位置から、6 自由度アームの幾何ヤコビ行列を1列ずつ組み立てる
- 可操作度を計算し、ヤコビ行列から肘・肩・手首の特異点を見分ける
- 特異点の近くで関節速度がどう増大するかを予測し、減衰最小二乗法で上限を抑える
- シミュレーション上のアームを目標位置姿勢まで動かす分解速度制御を実装する
順運動学は、ツールがどこにあるかを教えてくれます。本レッスンで扱うのは、関節が回ったときにツールがどれだけの速さで、どの方向に動くかです。関節速度からツール速度への写像がヤコビ行列です。ヤコビ行列は、速度制御、特異点のチェック、力の写像、そして数値的な逆運動学の大半の土台になっています。本レッスンは、回転行列を使いこなせていて、ヤコビ行列を導出し、検証し、アームの制御に使いたい読者を対象にしています。コードはすべてnumpyを使い、数秒で実行できます。
線形写像としてのヤコビ行列
ツールの位置を関節角の関数として x = f(q) と書きます。時間で微分すると、連鎖律から ẋ = J(q) q̇ が得られます。ここで J の (i, j) 成分は ∂fᵢ/∂qⱼ です。関節構成を固定すれば、この写像は関節速度について線形です。行列そのものはアームが動くにつれて変化し、興味深い振る舞いはすべてそこから生まれます。
物理的にも直接解釈できます。J の第 j 列は、関節 j を 1 rad/s で回し、他の関節をすべて止めたときに得られるツール速度です。この描像を覚えておいてください。本レッスン後半の空間アームのヤコビ行列は、これをもとに組み立てます。
平面2リンクアームを手で解く
ベース座標系のx–y平面内にあるアームを考えます。本レッスンでは全体を通してREP 103の座標軸（x前方、y左、z上、右手系）を使います。2つの関節はどちらもz軸まわりに回転します。リンク長は l₁ = 0.5 m、l₂ = 0.4 m です。先端位置は次のとおりです。
x = l1 cos q1 + l2 cos(q1 + q2)
y = l1 sin q1 + l2 sin(q1 + q2)各行を q₁ と q₂ で微分すると、ヤコビ行列が得られます（単位は m/rad）。
J(q) = [ -l1 sin q1 - l2 sin(q1 + q2) -l2 sin(q1 + q2) ]
[ l1 cos q1 + l2 cos(q1 + q2) l2 cos(q1 + q2) ]q₁ = 0 rad、q₂ = π/2 rad（90度）のとき、先端は (0.5, 0.4) m にあり、行列は次のようになります。
J = [ -0.4 -0.4 ]
[ 0.5 0.0 ]q̇ = (0.2, −0.1) rad/s を指令します。先端速度は ẋ = −0.4 × 0.2 + (−0.4) × (−0.1) = −0.04 m/s、ẏ = 0.5 × 0.2 = 0.10 m/s です。
列を幾何学的に見てみます。第1列 (−0.4, 0.5) m/rad は、ベースから先端へのベクトル (0.5, 0.4) m に垂直です。その長さは 0.640 m で、関節1から先端までの距離と同じです。関節2は (0.5, 0) m にあるので、先端は関節2から (0, 0.4) m 離れており、第2列は (−0.4, 0) m/rad です。どちらの場合も、列は z × r、つまり関節軸と、関節から先端へのベクトルとの外積に等しくなっています。この観察こそが、幾何ヤコビ行列の考え方のすべてです。
行列式は l₁ l₂ sin q₂ で、この姿勢では 0.2 m²/rad² です。アームが完全に伸びきったとき、または完全に折りたたまれたときにゼロになります。この点は後で改めて扱います。
有限差分による確認
手計算の導出では、符号を一つ間違えがちです。中心差分を使えば、その間違いを見つけられます。第 i 列は (f(q + h eᵢ) − f(q − h eᵢ)) / 2h で近似します。打ち切り誤差は h² に比例して小さくなり、丸め誤差はおおよそ 2.2 × 10⁻¹⁶ / h で大きくなります。h = 10⁻⁶ では両者が 10⁻¹⁰ 付近で釣り合うので、解析的なヤコビ行列と数値的なヤコビ行列は小数点以下約10桁まで一致するはずです。
import numpy as np
L1, L2 = 0.5, 0.4 # link lengths (m)
def fk_planar(q: np.ndarray) -> np.ndarray:
"""Tip position (x, y) in metres for joint angles q in radians."""
x = L1 * np.cos(q[0]) + L2 * np.cos(q[0] + q[1])
y = L1 * np.sin(q[0]) + L2 * np.sin(q[0] + q[1])
return np.array([x, y])
def jacobian_planar(q: np.ndarray) -> np.ndarray:
"""Hand-derived 2x2 Jacobian, units m/rad."""
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],
])
def numeric_jacobian(f, q: np.ndarray, h: float = 1e-6) -> np.ndarray:
"""Central finite differences, one column per joint."""
columns = []
for i in range(len(q)):
dq = np.zeros_like(q)
dq[i] = h
columns.append((f(q + dq) - f(q - dq)) / (2.0 * h))
return np.column_stack(columns)
q = np.array([0.0, np.pi / 2])
J = jacobian_planar(q)
q_dot = np.array([0.2, -0.1]) # rad/s
tip_velocity = J @ q_dot
rng = np.random.default_rng(0)
worst = 0.0
for _ in range(100):
q_rand = rng.uniform(-np.pi, np.pi, size=2)
diff = jacobian_planar(q_rand) - numeric_jacobian(fk_planar, q_rand)
worst = max(worst, float(np.max(np.abs(diff))))
print(f"det J = {np.linalg.det(J):.3f}, l1*l2*sin(q2) = {L1 * L2 * np.sin(q[1]):.3f}")
print(f"tip velocity = ({tip_velocity[0]:.3f}, {tip_velocity[1]:.3f}) m/s")
print(f"worst analytic vs numeric difference: {worst:.1e}")
print(f"agree: {worst < 1e-8}")最初の2行には det J = 0.200, l1*l2*sin(q2) = 0.200 と tip velocity = (-0.040, 0.100) m/s が出力され、手計算と一致します。100個のランダムな関節構成での最大差は 1e-10 のオーダーで（正確な桁は丸めに依存します）、最後の行には agree: True と出力されます。
空間アームの幾何ヤコビ行列
空間アームでは、ツールは並進速度 v（m/s）と角速度 ω（rad/s）を持ちます。これらを縦に並べると、6次元ベクトルであるツイストになります。本レッスンでは、ツイストを並進速度が先の (v, ω) とし、v はツール点の速度で、どちらもベース座標系で表します。幾何ヤコビ行列は、q̇ をこのツイストに写す 6 × n 行列です。
各列は、平面アームでの観察から導けます。関節 i の単位軸ベクトルを zᵢ、その軸上の任意の点を pᵢ（どちらもベース座標系）、ツール点を p とします。回転関節の列は (zᵢ × (p − pᵢ), zᵢ) です。直動関節では (zᵢ, 0) になります。スライドしてもツールは軸方向に動くだけで、回転しないからです。列を埋めるのに必要なのは順運動学だけで、記号微分は一切いりません。
「幾何」という言葉は、この行列を解析ヤコビ行列と区別するためのものです。解析ヤコビ行列は、オイラー角のような最小限の姿勢表現を微分したものです。両者の違いは角度の規約に依存する行列1つで、その行列はジンバルロックで特異になります。理由は回転の表現を参照してください。コントローラはほぼ常に幾何ヤコビ行列を使います。
テスト用アーム
ここでは市販品ではない架空の6 自由度（DoF）アームを使うので、データシートはありません。肩からツール点までのリーチは 0.85 m で、可搬重量はモデル化していません。関節1はベースのz軸まわりのヨーです。肩（関節2）はベースから 0.30 m 上にあり、y軸まわりのピッチです。続いて長さ 0.40 m の上腕があり、その先が肘（関節3、ピッチ）です。長さ 0.35 m の前腕の先は球面手首です。球面手首はx軸まわりのロール（関節4）、y軸まわりのピッチ（関節5）、x軸まわりのロール（関節6）からなり、3軸すべてが1点で交わります。ツール点は手首中心から 0.10 m 先にあります。q = 0 では、アームは +x 方向にまっすぐ伸びています。y軸まわりの右手の法則により、正のピッチはアームを下向きに傾けます。
角速度の行を数値的に確認するには、回転行列の微分が必要です。R(t) がベース座標系で角速度 ω で回転しているとき、Ṙ Rᵀ は ω の歪対称行列です。R の中心差分で Ṙ を求めると、非対角成分の3つの組から ω が得られます。
import numpy as np
def rot(axis: str, angle: float) -> np.ndarray:
c, s = np.cos(angle), np.sin(angle)
if axis == "x":
return np.array([[1.0, 0.0, 0.0], [0.0, c, -s], [0.0, s, c]])
if axis == "y":
return np.array([[c, 0.0, s], [0.0, 1.0, 0.0], [-s, 0.0, c]])
return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])
UNIT = {"x": np.array([1.0, 0.0, 0.0]), "y": np.array([0.0, 1.0, 0.0]), "z": np.array([0.0, 0.0, 1.0])}
# Each joint: (rotation axis in its own frame, offset from the previous joint in metres).
JOINTS = [
("z", np.array([0.0, 0.0, 0.0])), # 1 base yaw
("y", np.array([0.0, 0.0, 0.30])), # 2 shoulder pitch
("y", np.array([0.40, 0.0, 0.0])), # 3 elbow pitch
("x", np.array([0.35, 0.0, 0.0])), # 4 wrist roll
("y", np.array([0.0, 0.0, 0.0])), # 5 wrist pitch
("x", np.array([0.0, 0.0, 0.0])), # 6 tool roll
]
TOOL = np.array([0.10, 0.0, 0.0]) # tool point in the last frame (m)
def fk(q: np.ndarray) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
"""Tool position, tool rotation, joint axes and joint origins, all in the base frame."""
R = np.eye(3)
p = np.zeros(3)
axes, origins = [], []
for (axis, offset), q_i in zip(JOINTS, q):
p = p + R @ offset
axes.append(R @ UNIT[axis])
origins.append(p.copy())
R = R @ rot(axis, q_i)
return p + R @ TOOL, R, np.array(axes), np.array(origins)
def jacobian(q: np.ndarray) -> np.ndarray:
"""Geometric Jacobian: rows 0-2 linear velocity (m/s), rows 3-5 angular velocity (rad/s)."""
p_tool, _, axes, origins = fk(q)
J = np.zeros((6, len(q)))
for i in range(len(q)):
J[:3, i] = np.cross(axes[i], p_tool - origins[i])
J[3:, i] = axes[i]
return J
def numeric_jacobian(q: np.ndarray, h: float = 1e-6) -> np.ndarray:
_, R0, _, _ = fk(q)
J = np.zeros((6, len(q)))
for i in range(len(q)):
dq = np.zeros(len(q))
dq[i] = h
p_plus, R_plus, _, _ = fk(q + dq)
p_minus, R_minus, _, _ = fk(q - dq)
J[:3, i] = (p_plus - p_minus) / (2.0 * h)
S = ((R_plus - R_minus) / (2.0 * h)) @ R0.T # approximately skew(omega)
J[3:, i] = 0.5 * np.array([S[2, 1] - S[1, 2], S[0, 2] - S[2, 0], S[1, 0] - S[0, 1]])
return J
q = np.array([0.3, -0.5, 1.0, 0.4, 0.6, -0.2])
J = jacobian(q)
J_num = numeric_jacobian(q)
q_wrist = q.copy()
q_wrist[4] = 0.0
J_wrist = jacobian(q_wrist)
print(f"max |J - J_num| = {np.max(np.abs(J - J_num)):.1e}")
print(f"agree: {np.allclose(J, J_num, atol=1e-8)}")
print(f"rank at q5 = 0.6 rad: {np.linalg.matrix_rank(J)}")
print(f"rank at q5 = 0 rad: {np.linalg.matrix_rank(J_wrist)}")
print(f"columns 4 and 6 equal at q5 = 0: {np.allclose(J_wrist[:, 3], J_wrist[:, 5])}")最大差は 1e-10 のオーダーで、agree: True と出力されるはずです。ランクはテスト姿勢では6で、q₅ = 0 rad では5に下がり、最後の行には True と出力されます。手首のピッチがゼロのとき、関節4と関節6は同じ点を通る同じ軸を共有します。2つの列は同一になるので、ツール回転のある方向について、それを生み出せる関節がなくなります。
可操作度と特異姿勢
特異値分解 J = U Σ Vᵀ は、ある瞬間にアームが何をできるかを表します。ノルムが1の関節速度ベクトルは、ツール速度の楕円体に写ります。その半軸は U の列の方向を向き、長さは特異値 σ に等しくなります。特異点とは、少なくとも1つの σ がゼロになる関節構成です。そこでは楕円体がつぶれ、ツールが動けない方向が生じます。
可操作度 w = √det(J Jᵀ) は特異値の積です。古典的な文献は、「さらに読む」に挙げたYoshikawaの1985年の論文「Manipulability of Robotic Mechanisms」です。w はこの楕円体の体積に比例します。平面アームのヤコビ行列は正方行列なので、w は |det J| = 0.2 |sin q₂| m² となり、q₂ = 90度で最大値 0.2 m² をとります。空間アームでは、0〜2行目の単位が m/rad、3〜5行目が無次元なので、1つの w では単位が混ざります。並進ブロックと回転ブロックで別々に計算するか、並進の行を代表長さでスケーリングしてください。
この6 自由度アームには3種類の特異点があり、どの列が従属になるかを調べれば、それぞれを見つけられます。
- 肘: q₃ = 0 または π rad。前腕が上腕と一直線になり、手首中心はその直線に沿って動けなくなります。
- 肩: 手首中心が関節1の軸上にあります。このときベースをヨー回転させても、手首はその場で回るだけで移動しません。
- 手首: q₅ = 0 または π rad。上のコードで示したとおり、関節4と関節6の軸がそろいます。
特異点の近くで関節速度はどうなるか
q₁ = 0 として、平面アームの先端を x 方向に 0.1 m/s で動かすよう指令します。2 × 2 のヤコビ行列を手で逆算すると、q̇₁ = 0.2 cos q₂ / sin q₂ rad/s、q̇₂ = −(0.25 + 0.2 cos q₂) / sin q₂ rad/s となります。アームが伸びるにつれて、どちらも 1/sin q₂ で発散します。下のコードは2つのうち大きいほうを表にまとめ、減衰最小二乗解も加えています。減衰最小二乗解についてはグラフの後で説明します。
import numpy as np
L1, L2 = 0.5, 0.4 # link lengths (m)
def jacobian_planar(q: np.ndarray) -> np.ndarray:
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],
])
def dls(J: np.ndarray, v: np.ndarray, damping: float) -> np.ndarray:
"""Damped least-squares joint rates for a desired tip velocity v."""
return J.T @ np.linalg.solve(J @ J.T + damping**2 * np.eye(J.shape[0]), v)
v = np.array([0.1, 0.0]) # desired tip velocity (m/s), along x
for deg in [90, 60, 45, 30, 20, 10, 5, 2, 1]:
J = jacobian_planar(np.array([0.0, np.radians(deg)]))
w = np.sqrt(np.linalg.det(J @ J.T))
q_dot = np.linalg.solve(J, v)
print(f"q2 = {deg:3d} deg w = {w:.4f} m^2 max|q_dot| = {np.max(np.abs(q_dot)):7.3f} rad/s")
J = jacobian_planar(np.array([0.0, np.radians(1.0)]))
q_dot = dls(J, v, damping=0.05)
print(f"DLS at q2 = 1 deg: max|q_dot| = {np.max(np.abs(q_dot)):.3f} rad/s, "
f"achieved x speed = {(J @ q_dot)[0] * 1000:.1f} mm/s")期待される出力:
q2 = 90 deg w = 0.2000 m^2 max|q_dot| = 0.250 rad/s
q2 = 60 deg w = 0.1732 m^2 max|q_dot| = 0.404 rad/s
q2 = 45 deg w = 0.1414 m^2 max|q_dot| = 0.554 rad/s
q2 = 30 deg w = 0.1000 m^2 max|q_dot| = 0.846 rad/s
q2 = 20 deg w = 0.0684 m^2 max|q_dot| = 1.280 rad/s
q2 = 10 deg w = 0.0347 m^2 max|q_dot| = 2.574 rad/s
q2 = 5 deg w = 0.0174 m^2 max|q_dot| = 5.154 rad/s
q2 = 2 deg w = 0.0070 m^2 max|q_dot| = 12.891 rad/s
q2 = 1 deg w = 0.0035 m^2 max|q_dot| = 25.783 rad/s
DLS at q2 = 1 deg: max|q_dot| = 0.129 rad/s, achieved x speed = 0.5 mm/s0.1 m/s という控えめな先端速度でも、無理のない肘の角度なら 0.25 rad/s で済みますが、伸びきった状態から1度の位置では 25.8 rad/s が必要です。これは関節出力で約 246 rpm に相当します。ヤコビ行列を無条件に逆算するコントローラは、モータを飽和させるか、速度制限に引っかかるか、アームを激しく振り回すことになります。その見返りに、ツールはほとんど動きません。要求した方向が、速度楕円からほぼ消えているからです。
減衰最小二乗法
歴史的な経緯については、1986年の2本の論文を参照してください。NakamuraとHanafusaの「Inverse Kinematic Solutions With Singularity Robustness for Robot Manipulator Control」（「さらに読む」に掲載）と、WamplerのManipulator Inverse Kinematic Solutions Based on Vector Formulations and Damped Least-Squares Methodsです。減衰最小二乗解は ‖J q̇ − v‖² + λ² ‖q̇‖² を最小化します。結果は q̇ = Jᵀ (J Jᵀ + λ² I)⁻¹ v です。値 σ の特異方向に沿ったゲインは、1/σ ではなく σ / (σ² + λ²) になります。このゲインは 1/(2λ) を超えません。λ = 0.05 m では上限は指令変位 1 m あたり 10 rad なので、0.1 m/s の要求がその方向で 1 rad/s を超えることはありません。
1度の位置では、減衰解のピークは 0.129 rad/s で、先端は x 方向に 0.5 mm/s しか動きません。これがトレードオフです。減衰最小二乗法は、アームにできない動きをどんな代償を払ってでも指令するのではなく、その動きを引き受けません。特異点から遠ければ σ は λ よりずっと大きく、解は厳密な逆行列による解とほぼ同じになります。λ の選び方は工学的な判断です。最小特異値がしきい値を下回ったときだけ λ を大きくする方法もあります。
分解速度制御
1969年にWhitneyが示した分解速度制御は、関節角を解く代わりにツールの速度を指令して、ツールを動かします。各制御ステップで位置姿勢誤差を測り、それをツイストに変換し、ヤコビ行列を通じてそのツイストを関節速度に写し、積分します。
位置誤差は p_goal − p です。姿勢誤差は R_goal Rᵀ の回転ベクトルです。これは、現在のツール座標系を目標に重ねる回転を軸角度表現にし、ベース座標系で表したものです。ツイストを K e とし、ヤコビ行列の厳密な逆行列を使うと、連続時間ではどちらの誤差も ė = −K e に従います。誤差は指数関数的に減衰し、ツールはほぼ直線に沿って移動します。K = 2 s⁻¹ では、誤差は 0.5 s ごとに e 分の1になります。
import numpy as np
def rot(axis: str, angle: float) -> np.ndarray:
c, s = np.cos(angle), np.sin(angle)
if axis == "x":
return np.array([[1.0, 0.0, 0.0], [0.0, c, -s], [0.0, s, c]])
if axis == "y":
return np.array([[c, 0.0, s], [0.0, 1.0, 0.0], [-s, 0.0, c]])
return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])
UNIT = {"x": np.array([1.0, 0.0, 0.0]), "y": np.array([0.0, 1.0, 0.0]), "z": np.array([0.0, 0.0, 1.0])}
JOINTS = [
("z", np.array([0.0, 0.0, 0.0])),
("y", np.array([0.0, 0.0, 0.30])),
("y", np.array([0.40, 0.0, 0.0])),
("x", np.array([0.35, 0.0, 0.0])),
("y", np.array([0.0, 0.0, 0.0])),
("x", np.array([0.0, 0.0, 0.0])),
]
TOOL = np.array([0.10, 0.0, 0.0]) # m
def fk(q: np.ndarray) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
R = np.eye(3)
p = np.zeros(3)
axes, origins = [], []
for (axis, offset), q_i in zip(JOINTS, q):
p = p + R @ offset
axes.append(R @ UNIT[axis])
origins.append(p.copy())
R = R @ rot(axis, q_i)
return p + R @ TOOL, R, np.array(axes), np.array(origins)
def jacobian(q: np.ndarray) -> np.ndarray:
p_tool, _, axes, origins = fk(q)
J = np.zeros((6, len(q)))
for i in range(len(q)):
J[:3, i] = np.cross(axes[i], p_tool - origins[i])
J[3:, i] = axes[i]
return J
def rotation_log(R: np.ndarray) -> np.ndarray:
"""Rotation vector (axis times angle, rad). Accurate for small angles, not valid near pi."""
w = 0.5 * np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]])
s = np.linalg.norm(w) # sin(angle)
c = (np.trace(R) - 1.0) / 2.0 # cos(angle)
if s < 1e-12:
return np.zeros(3)
return np.arctan2(s, c) / s * w
def dls(J: np.ndarray, twist: np.ndarray, damping: float) -> np.ndarray:
return J.T @ np.linalg.solve(J @ J.T + damping**2 * np.eye(J.shape[0]), twist)
q = np.array([0.0, -0.4, 1.2, 0.0, 0.6, 0.0]) # start (rad)
q_goal = np.array([0.5, -0.2, 0.9, 0.3, 0.8, -0.4]) # defines a reachable goal pose (rad)
p_goal, R_goal, _, _ = fk(q_goal)
p_start, _, _, _ = fk(q)
gain = 2.0 # 1/s
dt = 0.01 # s, a 100 Hz control loop
damping = 0.01 # m
steps = 800 # 8 s
peak_speed = 0.0
for _ in range(steps):
p, R, _, _ = fk(q)
error = np.concatenate([p_goal - p, rotation_log(R_goal @ R.T)])
q_dot = dls(jacobian(q), gain * error, damping)
peak_speed = max(peak_speed, float(np.max(np.abs(q_dot))))
q = q + q_dot * dt
p, R, _, _ = fk(q)
pos_err = float(np.linalg.norm(p_goal - p))
rot_err = float(np.linalg.norm(rotation_log(R_goal @ R.T)))
print(f"initial position error: {np.linalg.norm(p_goal - p_start) * 1000:.0f} mm")
print(f"final position error: {pos_err:.1e} m")
print(f"final orientation error: {rot_err:.1e} rad")
print(f"peak joint speed: {peak_speed:.2f} rad/s")
print(f"converged: {pos_err < 1e-6 and rot_err < 1e-6}")最初の行には initial position error: 347 mm と出力されます。各ステップで誤差はおよそ 1 − K Δt = 0.98 倍になり、0.98⁸⁰⁰ は約 1e-7 です。したがって、最終誤差はどちらも 1e-8 から 1e-7 の間に収まります（単位はそれぞれ m と rad）。正確な桁は、丸めと、減衰によるわずかな減速に依存します。それでもどちらも 1e-6 のしきい値を十分に下回るので、最後の行には converged: True と出力されます。ピーク関節速度の行は、ゲインの代償を示しています。最初のステップでは指令関節速度が K に対して線形なので、ゲインを2倍にすると関節速度も2倍になります。
このループで実機を動かす前に、重要な点が3つあります。第一に、関節の速度制限と位置制限を守らせる必要があります。理想的には、ツールが方向を保つように q̇ ベクトル全体をスケーリングします。第二に、実際のコントローラは q̇ を一定周期で速度インタフェースに送り、自分の指令を積分するのではなく、計測した関節角を読み戻します。第三に、目標は作業領域の内側に収める必要があります。肩から 0.85 m より遠い位置姿勢を要求すると、ループはアームを肘の特異点に追い込み、関節速度を有界に保つのは減衰だけになります。その下で動くサーボループは制御のトピックハブで扱っています。速度レベルの制御をシミュレーションで試すには、2リンクアームで描くプロジェクトがよい出発点です。
演習
平面アームに長さ 0.2 m の第3リンクを加え、先端のヤコビ行列を 2 × 3 にしてください。手で導出し、numeric_jacobian と 1e-8 より良い精度で一致することを確認してください。次に、q = (0, 1.0, −0.5) rad で np.linalg.svd を使って零空間を求め、その零空間ベクトルに沿った関節速度では先端速度が 1e-12 m/s 未満にとどまることを数値的に示してください。最後に、分解速度制御のスクリプトで q_goal[4] を 0.05 rad に変更し、目標を手首の特異点の近くに置きます。減衰を 0.0 m と 0.05 m にして実行し、それぞれのピーク関節速度と最終誤差を記録してください。違いを2文で説明してください。
まとめ
- ヤコビ行列は関節速度をツール速度に写します。各列は、1つの関節が 1 rad/s で動いたときに生じるツールのツイストです。
- 回転関節では、幾何ヤコビ行列の列は (zᵢ × (p − pᵢ), zᵢ) で、順運動学だけから組み立てられます。h = 1e-6 の中心差分で、約 1e-10 の精度で検証できます。
- 特異値は速度楕円体を与えます。可操作度は特異値の積で、リンク長 0.5 m と 0.4 m の平面アームでは 0.2 |sin q₂| m² です。
- 特異点の近くでは、厳密な逆行列は 1/σ で増大する関節速度を要求します。先端速度 0.1 m/s では、伸びきった状態から1度の位置で 25.8 rad/s になります。減衰最小二乗法はゲインを 1/(2λ) に抑えます。
- 分解速度制御は、位置姿勢誤差をツイストに変換し、減衰付き逆行列で得た関節速度を積分します。特異点から離れていれば、速度 K で指数収束します。
さらに読む
- Velocity Kinematics and Statics。K. M. Lynch・F. C. Park著「Modern Robotics: Mechanics, Planning, and Control」（Cambridge University Press、2017年）の章です。著者らは書籍の無料プレプリントを公開しています。
- Robotics: Modelling, Planning and Control。B. Siciliano・L. Sciavicco・L. Villani・G. Oriolo著（Springer、2009年）。第3章「Differential Kinematics and Statics」が対応する内容です。
- Manipulability of Robotic Mechanisms。T. Yoshikawa著、The International Journal of Robotics Research、1985年。
- Resolved Motion Rate Control of Manipulators and Human Prostheses。D. E. Whitney著、IEEE Transactions on Man-Machine Systems、1969年。
- Inverse Kinematic Solutions With Singularity Robustness for Robot Manipulator Control。Y. Nakamura・H. Hanafusa著、Journal of Dynamic Systems, Measurement, and Control、1986年。